//***************************************************************************** // (C) Automotive Lighting Reutlingen GmbH // Tuebinger Strasse 123, 72762 Reutlingen, Germany // // Automotive Lighting Reutlingen GmbH owns all the rights to this work. // This work shall not be copied, reproduced, used, modified, transferred // or its information shall not be disclosed without the prior written // authorization of Automotive Lighting Reutlingen GmbH. //***************************************************************************** //----------------------------------------------------------------------------- /// \file CddStp8889A_StallDetection.c /// /// \brief Handling & WORKAROUND of Stepper-Stall-Detection of TI-HW DRV8889A /// /// \descr Handling of Stepper-Stall-Detection of TI-HW DRV8889A /// 'Workaround due to the fact of 'automatic'-Stall-Detection is NOT working the expected way /// /// HW-Registers concerning 'Stalling' /// Stat-Reg-'B' - 1 | 1 | UVLO | CPUV | OCP | STL | TF | OL - Basic-Status - comes each time 'automatically' /// Stat-Diag-'2' - UTW | OTW | OTS | STL_LRN_OK | STALL | RSVD | OL_B | OL_A - /// Ctrl-Reg-5 - RSVD [1:0] | STL_LRN | EN_STL | STL_REP | OL_TIME [1:0] | EN_SR_BLANK - /// Ctrl-Reg-6 - STALL_TH [7:0] - /// Ctrl-Reg-7 - TRQ_COUNT [7:0] - /// /// IsStalled: Pre-Condition: Ubatt & Temp is in valid range ('Environment ok') /// EnvOk: Pre-Condition: Coding-Param 'Enable' is TRUE /// CodTrue: Pre-Condition: Coding-Param are 'valid' /// CodValid: Pre-Condition: ReadOnce ('CddStp8889A_StallDetection_CodingData_bReadOnceDone') /// ReadOnce: Pre-Condition: None /// /// \author J. Kling ALRT-EES6 (f84304b) //----------------------------------------------------------------------------- #include #include #include #include #ifndef UNIT_TEST // NOT Unit-Tests = 'regular' #include #include #include #include // instead of RTE: 'IoHwAbUser_GetMountingSide', 'IoHwAbUser_SetDirLwr', 'IoHwAbUser_SetLwrSleep', 'IoHwAbUser_SetLwrDrvOff' #else // Unit-Tests! uint32 CddStp_UnitTestHelper_ul1; uint32 CddStp_UnitTestHelper_ul2; boolean CddStp_UnitTestHelper_bo1; boolean CddStp_UnitTestHelper_bo2; #define CFA_CNT_STP_88XX (4u) //#include // instead of RTE: 'IoHwAbUser_GetMountingSide', 'IoHwAbUser_SetDirLwr', 'IoHwAbUser_SetLwrSleep', 'IoHwAbUser_SetLwrDrvOff' //typedef unsigned char tieMountingSide; extern void IoHwAbUser_GetMountingSide(tieMountingSide* peVal); extern void IoHwAbUser_SetLwrDrvOff(boolean boValue); extern void IoHwAbUser_SetLwrSleep(boolean boValue); extern void IoHwAbUser_SetDirLwr(boolean boValue); //#include <../../Cust/Bw/Bw16/App/Bsw/Cdd/CddStp/CddStpPrj.h> #include #include // 'optional' Stall-Detection TI DRV8889A typedef struct { boolean bEN; uint8 ucSTALL_THRESHOLD; uint8 ucUBAT_MIN; uint8 ucUBAT_Max; uint8 ucLowTemp; uint8 ucHighTemp; uint8 ucPARAMETER_1; // reserved for future use uint8 ucPARAMETER_2; // reserved for future use uint8 ucPARAMETER_3; // reserved for future use uint8 ucPARAMETER_4; // reserved for future use uint8 ucPARAMETER_5; // reserved for future use uint8 ucPARAMETER_6; // reserved for future use } tNvmPara_StallDetection; #endif #ifdef CDDSTP_CFG_SIMULATION // defined in '.\Asr6\Src\Src_AL\Cust\Bw\Bw16\App\Bsw\Cdd\CddStp\CddStpPrj.h' #include #endif // CDDSTP_CFG_SIMULATION #include <8899/CddStp8899Cfg.h> #include <88XX/CddStp88XX_HalStp.h> #include <8889A/CddStp8889A_StallDetection.h> //============================================================================= // defines //============================================================================= //============================================================================= // local types //============================================================================= //============================================================================= // local function prototypes //============================================================================= STATIC_AL void CddStp8889A_StallDetection_CheckCoding(void); STATIC_AL boolean CddStp8889A_StallDetection_IsEnabled(void); //============================================================================= // variables //============================================================================= STATIC_AL tNvmPara_StallDetection CddStp8889A_StallDetection_CodingData; STATIC_AL boolean CddStp8889A_StallDetection_CodingData_bReadOnceDone; STATIC_AL boolean CddStp8889A_StallDetection_bEnabled; STATIC_AL uint8 CddStp8889A_StallDetection_ucStallCounter; STATIC_AL boolean CddStp8889A_StallDetection_bIsStalled; STATIC_AL volatile uint32 CddStp8889A_StallDetection_DEBUG_ulStallCounter01; STATIC_AL volatile uint32 CddStp8889A_StallDetection_DEBUG_ulStallCounter02; STATIC_AL volatile uint32 CddStp8889A_StallDetection_DEBUG_ulStallCounter03; //============================================================================= // constants //============================================================================= STATIC_AL /*const*/ uint8 CddStp8889A_StallDetection_ucStallCounter_Max = 2; // too slow: '5' //============================================================================= // Local functions implementation //============================================================================= //----------------------------------------------------------------------------- /// \brief CddStp8889A_StallDetection_CheckCoding /// /// \descr Check & read once Coding-Data /// /// \param - /// /// \return void //----------------------------------------------------------------------------- STATIC_AL void CddStp8889A_StallDetection_CheckCoding(void) { // available RTE - Fcts/Values // boolean Cod_Lwr_StallDetection_Enable (void) { return CddStp_tCodingArea_StallDetection.bEN; } // uint8 Cod_Lwr_StallDetection_StallThreshold (void) { return CddStp_tCodingArea_StallDetection.ucSTALL_THRESHOLD; } // uint8 Cod_Lwr_StallDetection_UBatMin (void) { return CddStp_tCodingArea_StallDetection.ucUBAT_MIN; } // uint8 Cod_Lwr_StallDetection_UBatMax (void) { return CddStp_tCodingArea_StallDetection.ucUBAT_Max; } // uint8 Cod_Lwr_StallDetection_LowTemp (void) { return CddStp_tCodingArea_StallDetection.ucLowTemp; } // uint8 Cod_Lwr_StallDetection_HighTemp (void) { return CddStp_tCodingArea_StallDetection.ucHighTemp; } // uint8 Cod_Lwr_StallDetection_Param1 (void) { return CddStp_tCodingArea_StallDetection.ucPARAMETER_1; } // reserved for future use // uint8 Cod_Lwr_StallDetection_Param2 (void) { return CddStp_tCodingArea_StallDetection.ucPARAMETER_2; } // reserved for future use // uint8 Cod_Lwr_StallDetection_Param3 (void) { return CddStp_tCodingArea_StallDetection.ucPARAMETER_3; } // reserved for future use // uint8 Cod_Lwr_StallDetection_Param4 (void) { return CddStp_tCodingArea_StallDetection.ucPARAMETER_4; } // reserved for future use // uint8 Cod_Lwr_StallDetection_Param5 (void) { return CddStp_tCodingArea_StallDetection.ucPARAMETER_5; } // reserved for future use // uint8 Cod_Lwr_StallDetection_Param6 (void) { return CddStp_tCodingArea_StallDetection.ucPARAMETER_6; } // reserved for future use tieDataStatus CodDataStat; (void)Rte_Read_ppareCodMCodingDataStatus0_eCompleteCodingDataStatus(&CodDataStat); if (CodDataStat == tieDataStatus_Valid) { CddStp8889A_StallDetection_CodingData_bReadOnceDone = TRUE; CddStp8889A_StallDetection_ResetStalled(); CddStp8889A_StallDetection_CodingData.bEN = Cod_Lwr_StallDetection_Enable(); CddStp8889A_StallDetection_CodingData.ucSTALL_THRESHOLD = Cod_Lwr_StallDetection_StallThreshold(); CddStp8889A_StallDetection_CodingData.ucUBAT_MIN = Cod_Lwr_StallDetection_UBatMin(); // [0.1 V] Spannungsschwelle fur CddStp8889A_StallDetection_CodingData.ucUBAT_Max = Cod_Lwr_StallDetection_UBatMax(); // [0.1 V] Spannungsschwelle fur CddStp8889A_StallDetection_CodingData.ucLowTemp = Cod_Lwr_StallDetection_LowTemp(); // [Deg + 40] Temperaturgrenzwert, CddStp8889A_StallDetection_CodingData.ucHighTemp = Cod_Lwr_StallDetection_HighTemp(); // [Deg + 40] Temperaturgrenzwert, // Reserve CddStp8889A_StallDetection_CodingData.ucPARAMETER_1 = Cod_Lwr_StallDetection_Param1(); CddStp8889A_StallDetection_CodingData.ucPARAMETER_2 = Cod_Lwr_StallDetection_Param2(); CddStp8889A_StallDetection_CodingData.ucPARAMETER_3 = Cod_Lwr_StallDetection_Param3(); CddStp8889A_StallDetection_CodingData.ucPARAMETER_4 = Cod_Lwr_StallDetection_Param4(); CddStp8889A_StallDetection_CodingData.ucPARAMETER_5 = Cod_Lwr_StallDetection_Param5(); CddStp8889A_StallDetection_CodingData.ucPARAMETER_6 = Cod_Lwr_StallDetection_Param6(); // temp if (CddStp8889A_StallDetection_CodingData.ucPARAMETER_1 > 0u) // already meaningful? { CddStp8889A_StallDetection_ucStallCounter_Max = CddStp8889A_StallDetection_CodingData.ucPARAMETER_1; } // workaround untill correct/valid data // if (CddStp8889A_StallDetection_CodingData.ucSTALL_THRESHOLD == 0u) // { // /* jk-tbd !!! */ CddStp8889A_StallDetection_CodingData.bEN = TRUE; /* jk-tbd !!! */ // /* jk-tbd !!! */ CddStp8889A_StallDetection_CodingData.ucSTALL_THRESHOLD = 0x20; /*=30*/ /* jk-tbd !!! */ // /* jk-tbd !!! */ CddStp8889A_StallDetection_CodingData.ucUBAT_MIN = 115; /* jk-tbd !!! */ // /* jk-tbd !!! */ CddStp8889A_StallDetection_CodingData.ucUBAT_Max = 145; /* jk-tbd !!! */ // /* jk-tbd !!! */ CddStp8889A_StallDetection_CodingData.ucLowTemp = 45; /* jk-tbd !!! */ // /* jk-tbd !!! */ CddStp8889A_StallDetection_CodingData.ucHighTemp = 90; /* jk-tbd !!! */ // } } else // coding (yet) not valid { CddStp8889A_StallDetection_CodingData_bReadOnceDone = FALSE; CddStp8889A_StallDetection_CodingData.bEN = FALSE; CddStp8889A_StallDetection_bEnabled = FALSE; CddStp8889A_StallDetection_ResetStalled(); } // PRQA S 5324 1 // program architecture and readability - intentionally no modification of existing code - warning - QAC(Prio2) Msg: 5324(HIS metrics violation: 'CddStp8889A_StallDetection_CheckCoding() : STCAL = 14' exceeds threshold 7 for Number of Distinct Function Calls (STCAL).) Depth: 0 Ref: } //----------------------------------------------------------------------------- /// \brief CddStp8889A_StallDetection_IsEnabled /// /// \descr Check if Stall-Detection is currently enabled /// * Check 'Referencing' (i.e. NO Stall-Detection during 'normal' movement) /// * Check Temperature-Range /// * Check Voltage-Range /// /// \param - /// /// \return boolean TRUE = Stall-Detection is currently enabled //----------------------------------------------------------------------------- STATIC_AL boolean CddStp8889A_StallDetection_IsEnabled(void) { boolean bIsEnabled = FALSE; if (CddStp8889A_StallDetection_CodingData_bReadOnceDone != TRUE) { CddStp8889A_StallDetection_CheckCoding(); } else // i.e.: (CddStp8889A_StallDetection_CodingData_bReadOnceDone == TRUE) { if( (CddStp8889A_StallDetection_bEnabled == TRUE) // dynamically enable? && (CddStp8889A_StallDetection_CodingData.bEN == TRUE)) // static enabled via coding? { uint16 unDIAG_VBAT = 0; uint8 ucQuelle = 0; uint8 ucTemperature = 0; tis32Temperature_mCelsius temperatureC = 0; CddStpVolt_IoHwAbUser_GetECUSupply(&unDIAG_VBAT); unDIAG_VBAT /= 100u; // e.g.: '13567' -> '135' // jk-tbd: should be somewhere in Coding-Data = CddStpData.c // tbd-FLM3 (void)Rte_Call_ppCodingData0_GetSTP_QUELLE_TT(&ucQuelle); // Source of temp.: 1..12=NTC1-12, 16=PCB, 17=CAN - SWRS.27423 ucQuelle = Cod_StpQuelleTT(); // = CDDSTP_ApplCodData_Sys.Stp_Source_LowTemp; / Source of temp.: 0 = Temperature measurement through the sensor of ECU | 1 = evaluation CAN-Signal | any other = reserved if (ucQuelle == 0u) // PCB-Temperature? { (void)Rte_Read_ppareRSensPcbTemperature0_lActual(&temperatureC); // e.g. '25' = '25,0'C' { } } // tbd-FLM3 else if (ucQuelle == 1u) // CAN-Temperature? // tbd-FLM3 { // tbd-FLM3 (void)Rte_Read_ppSigPrepATemp_lActual(&temperature) ; // CAN - SWRS.27697 - 23'C = '23' // tbd-FLM3 temperature = temperature / 1000; // tbd-FLM3 } else { // not supported temperatureC = 25; // 25'C } // FLM3: temperatureC += 40; // add 'Coding'-Offset: convert from 'sint' to 'uint8' - e.g. '25''C -> '65' ucTemperature = (uint8)temperatureC; if ( (unDIAG_VBAT >= CddStp8889A_StallDetection_CodingData.ucUBAT_MIN) // [0.1 V] Spannungsschwelle fur && (unDIAG_VBAT <= CddStp8889A_StallDetection_CodingData.ucUBAT_Max) // [0.1 V] Spannungsschwelle fur && (ucTemperature >= CddStp8889A_StallDetection_CodingData.ucLowTemp) // [Deg + 40] Temperaturgrenzwert, && (ucTemperature <= CddStp8889A_StallDetection_CodingData.ucHighTemp) // [Deg + 40] Temperaturgrenzwert, ) { bIsEnabled = TRUE; } } // end of 'if (CddStp8889A_StallDetection_CodingData_Enable == TRUE)' } // end of 'if (CddStp8889A_StallDetection_CodingData_bReadOnceDone = TRUE)' if (bIsEnabled == FALSE) { CddStp8889A_StallDetection_ucStallCounter = 0; // reset - maybe due to short voltage-drop } return bIsEnabled; } //============================================================================= // Public functions implementation //============================================================================= //----------------------------------------------------------------------------- /// \brief CddStp8889A_StallDetection_Enable /// /// \descr Enable dynamically the Stall-Detection /// 'TRUE' ONLY during 'Ref1' /// /// \param bDoEnable /// /// \return void //----------------------------------------------------------------------------- void CddStp8889A_StallDetection_Enable(boolean bDoEnable) { // changed? if (CddStp8889A_StallDetection_bEnabled != bDoEnable) { CddStp8889A_StallDetection_ResetStalled(); // reset anyway CddStp8889A_StallDetection_bEnabled = bDoEnable; } } //----------------------------------------------------------------------------- /// \brief CddStp8889A_StallDetection_IsStalled /// /// \descr Check if stepper is currently blocked due to evaluated 'Torque-Count' /// /// \param - /// /// \return boolean TRUE = Stepper is currently blocked due to evaluated 'Torque-Count' //----------------------------------------------------------------------------- boolean CddStp8889A_StallDetection_IsStalled(void) { boolean bIsStalled = FALSE; if (CddStp8889A_StallDetection_IsEnabled() == TRUE) // ... AND: 'CddStp88XX_aeState[ucIx] == CddStp88XX_eStateRef1/2' / 'CddStp88XX_aStpState[ucIx].Bit.RefActive == 1' { bIsStalled = CddStp8889A_StallDetection_bIsStalled; if ( (CddStp8889A_StallDetection_ucStallCounter == CddStp8889A_StallDetection_ucStallCounter_Max) // max reached? && (CddStp8889A_StallDetection_bIsStalled == FALSE)) // changed from '0' to '1' ? { CddStp8889A_StallDetection_bIsStalled = TRUE; CddStp88XX_aStpState[0].Bit.IsStalled = 1u; CddStp_Simulation_PreEvalStallDtctn_SetPosAtStall(CddStp88XX_HalStpFct[0].pGetActPos()); // -> 'CddStp88XX_HalStpLwr_GetActPos' -> 'CddStp88XX_HalStpLwr_GetActPos_' -> 'CddStp88XX_HalStp_StpInfoLwr.lActPos') // usually done in fct 'CddStp88XX_CtrlFct0' via cmd 'CddStp_eCtrlFct0_ModeOff' /* tbd-FLM3-new - done */ IoHwAbUser_SetLwrDrvOff(TRUE); // switch off driver - Write '1' to Hi-Z all outputs - OR'ed with SPI-Ctrl 'DIS_OUT' } } return bIsStalled; } //----------------------------------------------------------------------------- /// \brief CddStp8889A_StallDetection_ResetStalled /// /// \descr Reset 'Stalled'-State /// /// \param - /// /// \return void //----------------------------------------------------------------------------- void CddStp8889A_StallDetection_ResetStalled(void) { CddStp8889A_StallDetection_bIsStalled = FALSE; CddStp8889A_StallDetection_ucStallCounter = 0; CddStp88XX_aStpState[0].Bit.IsStalled = 0; } //----------------------------------------------------------------------------- /// \brief CddStp8889A_StallDetection_SetCurrentTrqCnt /// /// \descr Set current Torque-Count = Ctrl-Reg-7 /// /// \param ucTrqCnt Current Torque-Count = Ctrl-Reg-7 /// /// \return boolean TRUE = Stepper is currently blocked due to evaluated 'Torque-Count' //----------------------------------------------------------------------------- boolean CddStp8889A_StallDetection_SetCurrentTrqCnt(uint8 ucTrqCnt) { // normal/free running? if (ucTrqCnt > CddStp8889A_StallDetection_CodingData.ucSTALL_THRESHOLD) { CddStp8889A_StallDetection_ucStallCounter = 0; // reset } // else if (ucTrqCnt == CddStp8889A_StallDetection_CodingData_StallThreshold) // do NOT change counter 'intentionally' else // if (ucTrqCnt < CddStp8889A_StallDetection_CodingData_StallThreshold) // do incr counter { if (CddStp8889A_StallDetection_ucStallCounter < CddStp8889A_StallDetection_ucStallCounter_Max) { CddStp8889A_StallDetection_ucStallCounter++; } } // maybe 'Stalled'-State already 'locked' although 'free'-running return CddStp8889A_StallDetection_IsStalled(); } //----------------------------------------------------------------------------- /// \brief CddStp8889A_StallDetection_CheckStatReg /// /// \descr Incr. Stall-Det-DEBUG-Counter /// Store position at Stall-Detection /// /// \param - /// /// \return void //----------------------------------------------------------------------------- void CddStp8889A_StallDetection_CheckStatReg(void) { const uint8 SpiL2Channel = 0; static boolean bStallDtctn_STL_oldValue = FALSE; // indicates motor stall condition. boolean bStallDtctn_STL_newValue = CddStp8899Com_DataMirror.StatvRx.aSRegBasic[SpiL2Channel].Bit.u1STL == 1u ? TRUE : FALSE; if (CddStp8899Com_DataMirror.StatvRx.aSRegBasic[SpiL2Channel].Bit.u1STL != 0u) { CddStp8889A_StallDetection_DEBUG_ulStallCounter01++; } if (CddStp8899Com_DataMirror.StatvRx.aSRegFault[SpiL2Channel].Bit.u1STL != 0u) { CddStp8889A_StallDetection_DEBUG_ulStallCounter02++; } if (CddStp8899Com_DataMirror.StatvRx.aSRegDiag2[SpiL2Channel].Bit.u1STALL != 0u) { CddStp8889A_StallDetection_DEBUG_ulStallCounter03++; } // Bit 'STL' (=stall condition) changed from '0' to '1'? if ( (bStallDtctn_STL_oldValue == FALSE) && (bStallDtctn_STL_oldValue != bStallDtctn_STL_newValue)) { // stall-condition starts right NOW CddStp_Simulation_PreEvalStallDtctn_SetPosAtStall(CddStp88XX_HalStpFct[SpiL2Channel].pGetActPos()); // -> 'CddStp88XX_HalStpLwr_GetActPos' -> 'CddStp88XX_HalStpLwr_GetActPos_' -> 'CddStp88XX_HalStp_StpInfoLwr.lActPos') } bStallDtctn_STL_oldValue = bStallDtctn_STL_newValue; // store anyway // Pre-Eval of Stepper-Stall-Detection if (CddStp8899Com_DataMirror.StatvRx.aSRegDiag2[SpiL2Channel].Bit.u1STL_LRN_OK == 1u) { CddStp_Simulation_PreEvalStallDtctn_ResetStlLrn(); } } // EOF