//----------------------------------------------------------------------------- /// \file Afs.c /// /// \brief implementation of speed class for afs function /// /// \descr see doors for detail description /// /// \author f10035c /// mailTo:mario.wang@marelli.com /// 09.26.2022 /// /// Chen Gaven /// gaven.chen@external.marelli.com /// 05.25.2023 //----------------------------------------------------------------------------- // header files #include #include #include #include #include #include "Rte_Type.h" #undef RTE_APPLICATION_HEADER_FILE #include "Rte_ctaaAfs.h" #undef RTE_APPLICATION_HEADER_FILE // ***************************************************************************** // typedefs // ***************************************************************************** // ***************************************************************************** // vars // ***************************************************************************** #define ctaaAfs_START_SEC_CONST_UNSPECIFIED # include "ctaaAfs_MemMap.h" /* Used for Initilization of tisAfsFiltDebounce type variable data */ STATIC_AL const tisAfsFiltDebounce afs_sDataFiltDebounceInit = { 0u, 0u, 0u }; /* ------------------------------------------------------ */ /* CodM */ STATIC_AL const tRomPara_AFSConfigBlock* AFSCodConfig_Init = &applicationCodingData.AFSConfig; #define ctaaAfs_STOP_SEC_CONST_UNSPECIFIED # include "ctaaAfs_MemMap.h" #define ctaaAfs_START_SEC_VAR_ZERO_INIT_UNSPECIFIED # include "ctaaAfs_MemMap.h" //defined inputing signal STATIC_AL tisGeneralInfo afs_sGeneralInfo; STATIC_AL tisLightCmd afs_sLightCmd; STATIC_AL tisVehicleDynamic afs_sVehicleDynamic; #define ctaaAfs_STOP_SEC_VAR_ZERO_INIT_UNSPECIFIED # include "ctaaAfs_MemMap.h" #define ctaaAfs_START_SEC_VAR_INIT_UNSPECIFIED # include "ctaaAfs_MemMap.h" /* ------------------------------------------------------ */ /* Steer wheel Angle */ STATIC_AL uint16 afs_nInSteerwheelAngleData; STATIC_AL uint8 afs_ucInSteerwheelAngleQty; /* ------------------------------------------------------ */ /* Vehicle Speed */ STATIC_AL uint16 afs_unInVehicleSpeedData; STATIC_AL uint8 afs_ucInVehicleSpeedQty; /* Vehicle Gear */ STATIC_AL uint8 afs_ucInActlGearData; STATIC_AL uint8 afs_ucInActlGearQty; /* ------------------------------------------------------ */ /* Lower Beam Lamp */ STATIC_AL uint8 afs_ucInLowerBeamLampCmd; STATIC_AL uint8 afs_ucInLowerBeamLampQty; /* Light Knob */ STATIC_AL uint8 afs_ucInLightKnobData; STATIC_AL uint8 afs_ucInLightKnobQty; /* ------------------------------------------------------ */ /* Power Mode */ STATIC_AL uint8 afs_ucInPowerModeData; STATIC_AL uint8 afs_ucInPowerModeQty; /* ------------------------------------------------------ */ /* Debounce of steer wheel angle signal */ STATIC_AL tisAfsFiltDebounce afs_sSteerWheelAngleDebounce; STATIC_AL tieAfsTurnSteerState afs_eSteerWheelAngleState; /* Bend light output */ STATIC_AL tisCornerLight afs_sCornerLight; /* ------------------------------------------------------ */ /* Afs failsafe*/ STATIC_AL tisAFS_FS afs_Failsafe; /* ------------------------------------------------------ */ /* Debounce of AFS speed class */ STATIC_AL tisAfsFiltDebounce afs_sSpeedClassDebounce; STATIC_AL tisAfsOutput afs_sAfsOutput; const tRomPara_AFSModeCfg Afs_Config_Inactive = { 1, 0, 0, 0, 57, 0, 0, 0, 0, 0, {0, 0} }; /* ------------------------------------------------------ */ /* CodM mapping */ STATIC_AL tisAfsCfgMap AfsCfgMaptemp[CODMCFG_NUM] = { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } }; STATIC_AL uint8 SpeedClassHyst; STATIC_AL uint8 ArraySize; //The number of mode currently in use STATIC_AL uint8 AFSCodConfigQty; //CodM readiness flag /* ------------------------------------------------------ */ #define ctaaAfs_STOP_SEC_VAR_INIT_UNSPECIFIED # include "ctaaAfs_MemMap.h" // ***************************************************************************** // prototypes // ***************************************************************************** static void Afs_Get_Input(void); static void Afs_SteerWheelAngleBend(void); static void Afs_SpdClass_SteerWheelBend(uint8 mode); static void Afs_CheckEnableAfs(void); static void Afs_CheckSpeedClass(void); static void Afs_AfsOutput(void); static void Afs_WriteRte(void); static void Afs_ModeInit(void); static uint8 Afs_DataPendingDebounce(uint8 u8NewInputData, uint32 ulDebounceTime_In_MS, tisAfsFiltDebounce *tPtrDataActivedPending); static void Afs_SteerWheelAngleInit(void); # define RTE_START_SEC_CODE # include "ctaaAfs_MemMap.h" /// --------------------------------------------------------------------------- /// \func void Afs_Init(void) /// /// \brief speed class initialization /// /// \descr /// /// \param void /// /// \return void /// --------------------------------------------------------------------------- void Rte_riAfsInit(void) { afs_nInSteerwheelAngleData = 0; afs_ucInSteerwheelAngleQty = SIG_INVALID; afs_unInVehicleSpeedData = (uint16)0u; afs_ucInVehicleSpeedQty = SIG_INVALID; afs_ucInActlGearData = 0u; afs_ucInActlGearQty = SIG_INVALID; afs_ucInLowerBeamLampCmd = 0u; afs_ucInLowerBeamLampQty = SIG_INVALID; afs_ucInLightKnobData = 0u; afs_ucInLightKnobQty = SIG_INVALID; afs_ucInPowerModeData = 0u; afs_ucInPowerModeQty = SIG_INVALID; AFSCodConfigQty = SIG_INVALID; ArraySize = 0u; afs_Failsafe.eAFS = 0u; Afs_SteerWheelAngleInit(); Afs_ModeInit(); afs_sSpeedClassDebounce = afs_sDataFiltDebounceInit; afs_sAfsOutput.ucAfsClass = AFS_SPD_CLASS_INACTIVE; afs_sAfsOutput.ssStepOffset = SPD_CLASS_INACTIVE_STP_OFFSET; afs_sAfsOutput.boAfsEnable = FALSE; afs_sAfsOutput.eAfsQty = SIG_INVALID; Afs_WriteRte(); } static void Afs_SteerWheelAngleInit(void) { afs_sSteerWheelAngleDebounce = afs_sDataFiltDebounceInit; afs_eSteerWheelAngleState = TURN_STEER_NEUTRAL; afs_sCornerLight.boCornerLight0 = CLDisable; afs_sCornerLight.boCornerLight1 = CLDisable; afs_sCornerLight.eCornerLightQty = SIG_INVALID; } /// --------------------------------------------------------------------------- /// \func void Rte_rpAfs20ms(void) /// /// \brief SBL Main entry /// /// \param void /// /// \return void /// /// \descr main function of the Static Bending Light module /// --------------------------------------------------------------------------- void Rte_rpAfs20ms(void) { if (AFSCodConfig_Init->AFSGenCfg.AfsFunc_Enable) { Afs_Get_Input(); Afs_CheckEnableAfs(); Afs_CheckSpeedClass(); Afs_AfsOutput(); Afs_WriteRte(); } } /// --------------------------------------------------------------------------- /// \func Afs_Get_Input /// /// \brief Read data from RTE /// /// \param void /// /// \return void /// /// \descr Read from RTE speed, power status, LB status, steering wheel Angle /// --------------------------------------------------------------------------- static void Afs_Get_Input(void) { //Via RTE inputing signal Rte_Call_PpaclVehHubGeneralInfo_ReadData(&afs_sGeneralInfo); Rte_Call_PpaclVehHubLightCmd_ReadData(&afs_sLightCmd); Rte_Call_PpaclVehHubVehicleDynamic_ReadData(&afs_sVehicleDynamic); Rte_Read_PpareSysMonAFSFailSafe_sAFSFailsafe(&afs_Failsafe); afs_nInSteerwheelAngleData = afs_sVehicleDynamic.sSteerWheel.usAngelValue; afs_ucInSteerwheelAngleQty = afs_sVehicleDynamic.sSteerWheel.eQty; //AFS enable(a&b&c&d&e) afs_unInVehicleSpeedData = afs_sVehicleDynamic.sSpeed.usValue; afs_ucInVehicleSpeedQty = afs_sVehicleDynamic.sSpeed.eQty; afs_ucInActlGearData = afs_sVehicleDynamic.sActualGear.eValue; afs_ucInActlGearQty = afs_sVehicleDynamic.sActualGear.eQty; afs_ucInLowerBeamLampCmd = afs_sLightCmd.sLowerBeam.boValue; afs_ucInLowerBeamLampQty = afs_sLightCmd.sLowerBeam.eQty; afs_ucInLightKnobData = afs_sLightCmd.sLightSwitch.eStatus; afs_ucInLightKnobQty = afs_sLightCmd.sLightSwitch.eQty; afs_ucInPowerModeData = afs_sGeneralInfo.sPowerMode.eStatus; afs_ucInPowerModeQty = afs_sGeneralInfo.sPowerMode.eQty; } /// --------------------------------------------------------------------------- /// \func Afs_CheckSpeedClass /// /// \brief Afs mode /// /// \param void /// /// \return void /// /// \descr By the speed of the vechel, determine which mode it is in /// --------------------------------------------------------------------------- static void Afs_CheckSpeedClass(void) { uint8 ucSpeedClass_New; uint8 ucSpeedClass_Actived; uint8 i; ucSpeedClass_Actived = afs_sAfsOutput.ucAfsClass; ucSpeedClass_New = ucSpeedClass_Actived; if ((SigQtyValid == afs_ucInVehicleSpeedQty) && (TRUE == afs_sAfsOutput.boAfsEnable)) { for (i = 0; i < ArraySize ; i++) { //Greater than the speed upper threshold if ((AfsCfgMaptemp[ArraySize - i].tisAFSModeCfg->Speed_LowThreshold + SpeedClassHyst < afs_unInVehicleSpeedData) && (i != ArraySize - 1)) { ucSpeedClass_New = AfsCfgMaptemp[ArraySize - i].ucAFSModeClass; break; } //It is between the upper speed threshold and the lower speed threshold else if ((AfsCfgMaptemp[ArraySize - i].tisAFSModeCfg->Speed_LowThreshold - SpeedClassHyst <= afs_unInVehicleSpeedData) && (i != ArraySize - 1)) { if ((AfsCfgMaptemp[ArraySize - i].ucAFSModeClass != ucSpeedClass_Actived) && (AfsCfgMaptemp[ArraySize - i - 1].ucAFSModeClass != ucSpeedClass_Actived)) { ucSpeedClass_New = AfsCfgMaptemp[ArraySize - i].ucAFSModeClass; } break; } //Greater than minimum speed else if ((AfsCfgMaptemp[ArraySize - i].tisAFSModeCfg->Speed_LowThreshold < afs_unInVehicleSpeedData) && (i == ArraySize - 1)) { ucSpeedClass_New = AfsCfgMaptemp[ArraySize - i].ucAFSModeClass; break; } } if (SPD_CLASS_ZERO == afs_unInVehicleSpeedData) { ucSpeedClass_New = AFS_SPD_CLASS_INACTIVE; } } else { ucSpeedClass_New = AFS_SPD_CLASS_INACTIVE; } afs_sAfsOutput.ucAfsClass = Afs_DataPendingDebounce(ucSpeedClass_New, SPEED_CLASS_DEBOUNCE_TIME, &afs_sSpeedClassDebounce); } /// --------------------------------------------------------------------------- /// \func Afs_SteerWheelAngleBend /// /// \brief CornerLight Cmd /// /// \param void /// /// \return void /// /// \descr In SBL enable mode, the corresponding Angle control command is sent /// --------------------------------------------------------------------------- static void Afs_SteerWheelAngleBend(void) { if (SigQtyValid == afs_ucInSteerwheelAngleQty) { afs_sCornerLight.eCornerLightQty = SigQtyValid; if ((AfsCfgMaptemp[afs_sAfsOutput.ucAfsClass].tisAFSModeCfg->SBLEnable) && (AfsCfgMaptemp[afs_sAfsOutput.ucAfsClass].ucAFSModeClass == afs_sAfsOutput.ucAfsClass)) { Afs_SpdClass_SteerWheelBend(afs_sAfsOutput.ucAfsClass); } else { /* SBL Disable : Then Re-init filter and output */ afs_sSteerWheelAngleDebounce = afs_sDataFiltDebounceInit; afs_eSteerWheelAngleState = TURN_STEER_NEUTRAL; } /* TurnSteer ---> LeftCornerLightCmd or RightCornerLightCmd */ if(TURN_STEER_LEFT == afs_eSteerWheelAngleState) { afs_sCornerLight.boCornerLight0 = CLEnable; afs_sCornerLight.boCornerLight1 = CLDisable; } else if(TURN_STEER_RIGHT == afs_eSteerWheelAngleState) { afs_sCornerLight.boCornerLight0 = CLDisable; afs_sCornerLight.boCornerLight1 = CLEnable; } else { afs_sCornerLight.boCornerLight0 = CLDisable; afs_sCornerLight.boCornerLight1 = CLDisable; } } else { /* Signal invalid, then re-init */ Afs_SteerWheelAngleInit(); } } /// --------------------------------------------------------------------------- /// \func Afs_SpdClass_SteerWheelBend /// /// \brief Parsing the steering wheel Angle /// /// \param Afs mode /// /// \return void /// /// \descr Judging the deflection direction of the body from the steering wheel Angle /// --------------------------------------------------------------------------- static void Afs_SpdClass_SteerWheelBend(uint8 mode) { tieAfsTurnSteerState eNewTurnSteer = afs_eSteerWheelAngleState; if (AfsCfgMaptemp[mode].tisAFSModeCfg->SteeringAngle_On_Left + SBL_NEUTRAL_DEGREE_RAW_SIGNAL <= afs_nInSteerwheelAngleData) { eNewTurnSteer = TURN_STEER_LEFT; } else if (AfsCfgMaptemp[mode].tisAFSModeCfg->SteeringAngle_Off_Left + SBL_NEUTRAL_DEGREE_RAW_SIGNAL < afs_nInSteerwheelAngleData) { if ((TURN_STEER_RIGHT != afs_eSteerWheelAngleState) && (TURN_STEER_LEFT != afs_eSteerWheelAngleState)) { /* Right ---> Neutral */ eNewTurnSteer = TURN_STEER_NEUTRAL; } else { /* Keep be NEUTRAL or LEFT */ } } else if (SBL_NEUTRAL_DEGREE_RAW_SIGNAL - AfsCfgMaptemp[mode].tisAFSModeCfg->SteeringAngle_On_Right >= afs_nInSteerwheelAngleData) { eNewTurnSteer = TURN_STEER_RIGHT; } else if (SBL_NEUTRAL_DEGREE_RAW_SIGNAL - AfsCfgMaptemp[mode].tisAFSModeCfg->SteeringAngle_Off_Right > afs_nInSteerwheelAngleData) { if ((TURN_STEER_RIGHT != afs_eSteerWheelAngleState) && (TURN_STEER_LEFT != afs_eSteerWheelAngleState)) { /* Left ---> Neutral */ eNewTurnSteer = TURN_STEER_NEUTRAL; } else { /* Keep be NEUTRAL or RIGHT */ } } else { /* Right absolute */ eNewTurnSteer = TURN_STEER_NEUTRAL; } afs_eSteerWheelAngleState = (tieAfsTurnSteerState)Afs_DataPendingDebounce((uint8)eNewTurnSteer, TURN_STEER_DEBOUNCE_TIME, &afs_sSteerWheelAngleDebounce); } /// --------------------------------------------------------------------------- /// \func Afs_DataPendingDebounce /// /// \brief /// /// \param u8NewInputData,ulDebounceTime_In_MS,tPtrDataActivedPending /// /// \return void /// /// \descr Timer Debounce for variable data pending /// --------------------------------------------------------------------------- static uint8 Afs_DataPendingDebounce(uint8 u8NewInputData, uint32 ulDebounceTime_In_MS, tisAfsFiltDebounce *tPtrDataActivedPending) { /* temporary Data determined, checking with current Data and debounce if necessary */ if (u8NewInputData == tPtrDataActivedPending->ucActivedData) { /* Data not change */ tPtrDataActivedPending->ulFiltTimer = 0uL; /* reset debounce timer */ tPtrDataActivedPending->ucPendingData = u8NewInputData; } else if (u8NewInputData == tPtrDataActivedPending->ucPendingData) { /* check debounce timeout */ #if AFS_NORMAL_TIMER if ( TimerTimeout(tPtrDataActivedPending->ulFiltTimer) ) #else if ((uint32)0uL == tPtrDataActivedPending->ulFiltTimer) #endif { /* timeout */ tPtrDataActivedPending->ucActivedData = tPtrDataActivedPending->ucPendingData; tPtrDataActivedPending->ulFiltTimer = 0uL; } else { /* keep debouncing */ } } else { /* temporary Data != pending Data, start a new debounce timer */ #if AFS_NORMAL_TIMER tPtrDataActivedPending->ulFiltTimer = TimerStart(UTILTIME_MSEC(ulDebounceTime_In_MS)); #else tPtrDataActivedPending->ulFiltTimer = ulDebounceTime_In_MS; #endif tPtrDataActivedPending->ucPendingData = u8NewInputData; } /* output the data after debounce */ return(tPtrDataActivedPending->ucActivedData); } /// --------------------------------------------------------------------------- /// \func Afs_CheckEnableAfs /// /// \brief Afs enable /// /// \param void /// /// \return void /// /// \descr Check whether AFS is enabled based on the conditions /// --------------------------------------------------------------------------- static void Afs_CheckEnableAfs(void) { if ((SigQtyValid == afs_ucInPowerModeQty) && \ (SigQtyValid == afs_ucInLightKnobQty) && \ (SigQtyValid == afs_ucInLowerBeamLampQty) && \ (SigQtyValid == afs_ucInActlGearQty) && \ (SigQtyValid == afs_ucInVehicleSpeedQty) && \ (SigQtyValid == AFSCodConfigQty)) { afs_sAfsOutput.eAfsQty = SigQtyValid; if ((Power_ON == afs_ucInPowerModeData) && \ (LightKnob_Auto == afs_ucInLightKnobData) && \ (Switch_ON == afs_ucInLowerBeamLampCmd) && \ (SigQtyValid == afs_Failsafe.eAFS) && \ ((1u <= afs_ucInActlGearData) && (3u >= afs_ucInActlGearData))) { afs_sAfsOutput.boAfsEnable = TRUE; } else { afs_sAfsOutput.boAfsEnable = FALSE; } } else { afs_sAfsOutput.eAfsQty = SIG_INVALID; afs_sAfsOutput.boAfsEnable = FALSE; } } /// --------------------------------------------------------------------------- /// \func Afs_AfsOutput /// /// \brief Afs Output /// /// \param void /// /// \return void /// /// \descr Output Afs status /// --------------------------------------------------------------------------- static void Afs_AfsOutput(void) { if(FALSE != afs_sAfsOutput.boAfsEnable) { /* AFS function enable */ if (ArraySize >= afs_sAfsOutput.ucAfsClass) { afs_sAfsOutput.ssStepOffset = -((sint16)AfsCfgMaptemp[afs_sAfsOutput.ucAfsClass].tisAFSModeCfg->LWR_Offset); } /* Bend light process */ Afs_SteerWheelAngleBend(); } else { /* AFS function disable */ afs_sAfsOutput.ssStepOffset = SPD_CLASS_INACTIVE_STP_OFFSET; /* Bend light process */ Afs_SteerWheelAngleInit(); } } /// --------------------------------------------------------------------------- /// \func Afs_WriteRte /// /// \brief WriteRte /// /// \param void /// /// \return void /// /// \descr WriteRte /// --------------------------------------------------------------------------- static void Afs_WriteRte(void) { Rte_Write_PpaseAFSAfsOutput_sValue(&afs_sAfsOutput); Rte_Write_PpaseAFSCornerLight_sValue(&afs_sCornerLight); } /// --------------------------------------------------------------------------- /// \func Afs_ModeInit /// /// \brief CodM mapping /// /// \param void /// /// \return void /// /// \descr Map code in the codM to this afs module /// --------------------------------------------------------------------------- static void Afs_ModeInit(void) { tisCodingStatus CodingStatus; uint8 i; uint8 j = 0; (void)Rte_Read_PpareCodMCodingStatus_sCodingStatus(&CodingStatus); if (CodingStatus.eCodingStatus == E_OK) { tisAfsCfgMap AfsCfgMap[] = { { AFS_SPD_CLASS_INACTIVE, &Afs_Config_Inactive }, { AFS_SPD_CLASS_V1, &applicationCodingData.AFSConfig.AFSMode_V1 }, { AFS_SPD_CLASS_V2, &applicationCodingData.AFSConfig.AFSMode_V2 }, { AFS_SPD_CLASS_V3, &applicationCodingData.AFSConfig.AFSMode_V3 }, { AFS_SPD_CLASS_C1, &applicationCodingData.AFSConfig.AFSMode_C1 }, { AFS_SPD_CLASS_C2, &applicationCodingData.AFSConfig.AFSMode_C2 }, { AFS_SPD_CLASS_C3, &applicationCodingData.AFSConfig.AFSMode_C3 }, { AFS_SPD_CLASS_E1, &applicationCodingData.AFSConfig.AFSMode_E1 }, { AFS_SPD_CLASS_E2, &applicationCodingData.AFSConfig.AFSMode_E2 }, { AFS_SPD_CLASS_E3, &applicationCodingData.AFSConfig.AFSMode_E3 } }; for (i = 0; i < CODMCFG_NUM; i++) { if (AfsCfgMap[i].tisAFSModeCfg->ModeEnable) { AfsCfgMaptemp[j] = AfsCfgMap[i]; j++; } } ArraySize = j - 1; SpeedClassHyst = AFSCodConfig_Init->AFSGenCfg.Speed_UpHysteresis; AFSCodConfigQty = SigQtyValid; } else { AFSCodConfigQty = SIG_INVALID; } } # define RTE_STOP_SEC_CODE # include "ctaaAfs_MemMap.h" // EOF