mirror of
https://github.com/Dev-KATECH/ADM.git
synced 2026-05-17 01:43:59 +09:00
3minbe
99ad012576
- Drive_Mode.c 수정 : RC - ACU handover 전환 로직 보완 : RC -> ACU / ACU -> RC 전환 시 5km/h 이하에서만 Permit 되도록 수정 : 5km/h 초과 시 Handover Phase가 In Progress가 되도록 수정 : handover 속도 조건에 차속 피드백 외 제어 명령 속도 조건 추가 : ACU -> RC 전환 시 RC 속도 명령 5km/h 이하 조건 추가 : RC -> ACU 전환 시 ACU 속도 명령 5km/h 이하 조건 추가 : handover 진행 중 요청/제어 주체 해제 시 비상정지 진입 조건 보완 : ACU -> RC 진행 중 ACU Drive Mode 해제 시 Emergency 진입 : RC -> ACU 진행 중 RC ACU 요청 해제 시 Emergency 진입 : 관련 Handover/Emergency 상태 변수 설정 보완 - Drive_Mode.c/ExecuteEmergencyMode 함수 수정 : Handover conflict 발생 후 RC manual 요청으로 복구 가능하도록 조건 보완
685 lines
18 KiB
C
685 lines
18 KiB
C
#include "System_Layer/Drive_Mode.h"
|
||
#include "System_Layer/CheckTime/CheckTime.h"
|
||
|
||
|
||
static bool IsVehicleStopped(void)
|
||
{
|
||
return (GV_VCU_Vx_Fbk < 1.0f);
|
||
}
|
||
|
||
static bool IsVehicleBelowHandoverSpeed(void)
|
||
{
|
||
return (GV_VCU_Vx_Fbk <= 5.0f);
|
||
}
|
||
|
||
static bool IsRcHandoverReady(void)
|
||
{
|
||
return IsVehicleBelowHandoverSpeed() && (GV_RC_Vx_Command <= 5U);
|
||
}
|
||
|
||
static bool IsAcuHandoverReady(void)
|
||
{
|
||
return IsVehicleBelowHandoverSpeed() && (GV_ACU_Vx_Command <= 5U);
|
||
}
|
||
|
||
static bool HasAcuRequest(void)
|
||
{
|
||
return (GV_ACU_Drive_mode == 1U);
|
||
}
|
||
|
||
static bool IsRcParkRequest(void)
|
||
{
|
||
return (GV_RC_GearSelStat == 0U);
|
||
}
|
||
|
||
static uint8_t s_prevRcManualOn = 0U;
|
||
static uint8_t s_prevRcAutoOn = 0U;
|
||
static RcModeReq_e s_lastRcModeReq = RC_REQ_DEFAULT;
|
||
static RcModeReq_e s_rcModeReqRaw = RC_REQ_NONE;
|
||
static bool s_acuToRcHandoverActive = false;
|
||
static bool s_rcToAcuHandoverActive = false;
|
||
static bool s_acuToRcSourceSeen = false;
|
||
static bool s_rcToAcuSourceSeen = false;
|
||
|
||
static DriveMode_e curMode = DRIVE_MODE_DEFAULT;
|
||
static DriveMode_e nextMode = DRIVE_MODE_DEFAULT;
|
||
static HandoverPhase_e nextPhase = HO_NONE;
|
||
|
||
static bool IsRcRequestCompleted(RcModeReq_e request, DriveMode_e mode)
|
||
{
|
||
return ((request == RC_REQ_DEFAULT) && (mode == DRIVE_MODE_DEFAULT)) ||
|
||
((request == RC_REQ_MANUAL) && (mode == DRIVE_MODE_RC)) ||
|
||
((request == RC_REQ_AUTO) && (mode == DRIVE_MODE_ACU));
|
||
}
|
||
|
||
|
||
void CheckIgnition(void)
|
||
{
|
||
CheckSystemFault();
|
||
|
||
switch (GV_IG_Process_Status)
|
||
{
|
||
// ----- 0 ----- //
|
||
case KEY_STATE_OFF_COMPLETE:
|
||
SET_SW_VER();
|
||
Soc_Check_befor_IGN(); // Check SoC
|
||
|
||
if (GV_VCU_Ignition == 1 && GV_BMS_Fault_Check == 0 && GV_HV_Apply_Check == 0)
|
||
{
|
||
GV_VCU_Ready = 1;
|
||
GV_CE_ON_CMD = 1;
|
||
GV_IG_Process_Status = KEY_STATE_ON_PROCESS;
|
||
}
|
||
break;
|
||
|
||
// ----- 1 ----- //
|
||
case KEY_STATE_ON_PROCESS:
|
||
// Request init status from ECUs, Check if the ECUs are Ready
|
||
GV_MCU_Init_Status = (GV_CE_ON_CMD == 1);
|
||
GV_Steer_Init_Status = (GV_CE_ON_CMD == 1);
|
||
GV_Brake_Init_Status = (GV_CE_ON_CMD == 1);
|
||
GV_ACU_Init_Status = (GV_CE_ON_CMD == 1);
|
||
GV_All_ECU_Ready_Flag = (GV_BCM_Init_Status == 1 && GV_MCU_Init_Status == 1 && GV_Steer_Init_Status == 1 && GV_Brake_Init_Status == 1 && GV_ACU_Init_Status == 1);
|
||
|
||
if (GV_All_ECU_Ready_Flag == 1) // When all ECUs are ready, switch to 'Key on complete' state
|
||
{
|
||
GV_EPAM_CMD = 0; // EPAM Release Cmd
|
||
GV_IG_Process_Status = KEY_STATE_ON_COMPLETE;
|
||
}
|
||
else if (GV_VCU_Ignition == 0) // If the key is turned off during starting, it switches to 'key off process' state
|
||
GV_IG_Process_Status = KEY_STATE_OFF_PROCESS;
|
||
break;
|
||
|
||
// ----- 2 ----- //
|
||
case KEY_STATE_ON_COMPLETE:
|
||
ExecuteDriveMode();
|
||
Motor_Status();
|
||
Est_Vehicle_Speed();
|
||
|
||
if (GV_VCU_Ignition == 0 && (GV_VCU_Drive_Mode == DRIVE_MODE_DEFAULT || GV_VCU_Drive_Mode == DRIVE_MODE_EMERGENCY)) // If the key is turned off, it switches to 'key off process' state
|
||
GV_IG_Process_Status = KEY_STATE_OFF_PROCESS;
|
||
break;
|
||
|
||
// ----- 3 ----- //
|
||
case KEY_STATE_OFF_PROCESS:
|
||
CNT_GV_HV_ON_CMD = 0; // Reset counter
|
||
GV_ALL_ECU_Off_Flag = 1;
|
||
|
||
GV_IG_Process_Status = KEY_STATE_OFF_PROCESS_WAIT_HV;
|
||
break;
|
||
|
||
case KEY_STATE_OFF_PROCESS_WAIT_HV: // Wait for high voltage cutoff
|
||
HighVoltageOff();
|
||
break;
|
||
|
||
case KEY_STATE_OFF_PROCESS_WAIT_LDC: // Wait for LDC voltage drop
|
||
if (GV_LDC_INP_VOLT < 5) // Check the LDC voltage, if the LDC voltage falls below 5V, it switches to 'key off complete' state
|
||
{
|
||
GV_CE_ON_CMD = 0;
|
||
GV_IG_Process_Status = KEY_STATE_OFF_COMPLETE;
|
||
}
|
||
break;
|
||
|
||
default: // Exception
|
||
GV_IG_Process_Status = KEY_STATE_OFF_COMPLETE;
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
void ExecuteDriveMode()
|
||
{
|
||
SetVehicleEnvironment();
|
||
CheckDriveMode();
|
||
// LongitudinalCtrlMode();
|
||
|
||
Input_Data_Set_Logic();
|
||
ADM_Integrated_Logic_step();
|
||
Output_Data_Set_Logic();
|
||
|
||
switch(GV_VCU_Drive_Mode)
|
||
{
|
||
case DRIVE_MODE_ACU:
|
||
ExecuteACUMode();
|
||
break;
|
||
case DRIVE_MODE_RC:
|
||
ExecuteRCMode();
|
||
break;
|
||
case DRIVE_MODE_EMERGENCY:
|
||
ExecuteEmergencyMode();
|
||
break;
|
||
case DRIVE_MODE_DEFAULT:
|
||
ExecuteDefaultMode();
|
||
break;
|
||
}
|
||
|
||
GV_VCU_Drive_Mode = (uint8_t)nextMode;
|
||
GV_VCU_HandoverPhase = nextPhase;
|
||
//GV_RC_BrakeTorqueCommand_Master = GV_RC_BrakeTorqueCommand;
|
||
}
|
||
|
||
|
||
void CheckDriveMode(void)
|
||
{
|
||
RcModeReq_e requestedRcMode = RC_REQ_NONE;
|
||
bool autoWaitOn = false;
|
||
|
||
curMode = (DriveMode_e)GV_VCU_Drive_Mode;
|
||
nextMode = curMode;
|
||
// nextPhase = HO_NONE;
|
||
|
||
RcRequestCheck();
|
||
|
||
if(isEmergency)
|
||
{
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
|
||
nextPhase = HO_HOLD; // Reset handover phase
|
||
if(isEmergencyRc) GV_VCU_EmergencyCause = EMG_REQ_RC;
|
||
else if(isEmergencyAcu) GV_VCU_EmergencyCause = EMG_REQ_ACU;
|
||
else if(isEmergencySwitch) GV_VCU_EmergencyCause = EMG_REQ_EXTERNAL_SW;
|
||
else if(isEmergencyVcu) GV_VCU_EmergencyCause = EMG_REQ_VCU;
|
||
|
||
ACU_Arc_check();
|
||
}
|
||
else if(isCharging) //Author : JaeminSong, Date : 0830, Description: If Current_Mode is Charging, We need to change default Mode.
|
||
{
|
||
nextMode = DRIVE_MODE_DEFAULT;
|
||
|
||
GV_Charging_Status_M_Torq = 0;
|
||
GV_Motor_Torque_Cmd = GV_Charging_Status_M_Torq;
|
||
GV_Charging_Status_Steer_Angle = 0;
|
||
|
||
GV_VCU_EmergencyCause = EMG_NONE;
|
||
}
|
||
else
|
||
{
|
||
requestedRcMode = s_rcModeReqRaw;
|
||
|
||
if (s_acuToRcHandoverActive &&
|
||
(curMode == DRIVE_MODE_ACU) &&
|
||
!IsRcHandoverReady() &&
|
||
!HasAcuRequest())
|
||
{
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
nextPhase = HO_HOLD;
|
||
GV_VCU_EmergencyCause = EMG_HO_CONFLICT;
|
||
GV_RC_ModeReq = s_rcModeReqRaw;
|
||
Time_SetAutoOnWait(false);
|
||
s_acuToRcHandoverActive = false;
|
||
s_acuToRcSourceSeen = false;
|
||
return;
|
||
}
|
||
|
||
if (s_rcToAcuHandoverActive &&
|
||
(curMode == DRIVE_MODE_RC) &&
|
||
!IsAcuHandoverReady() &&
|
||
(GV_RC_ACU_Drive_Mode == 0U))
|
||
{
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
nextPhase = HO_HOLD;
|
||
GV_VCU_EmergencyCause = EMG_HO_CONFLICT;
|
||
GV_RC_ModeReq = s_rcModeReqRaw;
|
||
Time_SetAutoOnWait(false);
|
||
s_rcToAcuHandoverActive = false;
|
||
s_rcToAcuSourceSeen = false;
|
||
return;
|
||
}
|
||
|
||
if (requestedRcMode != RC_REQ_MANUAL)
|
||
{
|
||
s_acuToRcHandoverActive = false;
|
||
s_acuToRcSourceSeen = false;
|
||
}
|
||
|
||
if (requestedRcMode != RC_REQ_AUTO)
|
||
{
|
||
s_rcToAcuHandoverActive = false;
|
||
s_rcToAcuSourceSeen = false;
|
||
}
|
||
|
||
if ((curMode == DRIVE_MODE_EMERGENCY) &&
|
||
(GV_VCU_EmergencyCause == EMG_HO_SPD_COND_FAIL) &&
|
||
(requestedRcMode != RC_REQ_MANUAL))
|
||
{
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
nextPhase = HO_HOLD;
|
||
GV_RC_ModeReq = s_rcModeReqRaw;
|
||
Time_SetAutoOnWait(false);
|
||
return;
|
||
}
|
||
|
||
if ((curMode == DRIVE_MODE_EMERGENCY) &&
|
||
(GV_VCU_EmergencyCause == EMG_HO_CONFLICT) &&
|
||
(requestedRcMode != RC_REQ_DEFAULT) &&
|
||
(requestedRcMode != RC_REQ_MANUAL))
|
||
{
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
nextPhase = HO_HOLD;
|
||
GV_RC_ModeReq = s_rcModeReqRaw;
|
||
Time_SetAutoOnWait(false);
|
||
return;
|
||
}
|
||
|
||
if (!((curMode == DRIVE_MODE_EMERGENCY) &&
|
||
(((GV_VCU_EmergencyCause == EMG_HO_SPD_COND_FAIL) && (requestedRcMode != RC_REQ_MANUAL)) ||
|
||
((GV_VCU_EmergencyCause == EMG_HO_CONFLICT) &&
|
||
(requestedRcMode != RC_REQ_DEFAULT) &&
|
||
(requestedRcMode != RC_REQ_MANUAL)))))
|
||
GV_VCU_EmergencyCause = EMG_NONE;
|
||
|
||
switch (requestedRcMode)
|
||
{
|
||
case RC_REQ_NONE:
|
||
nextMode = curMode;
|
||
nextPhase = HO_NONE;
|
||
break;
|
||
|
||
case RC_REQ_DEFAULT:
|
||
if ((curMode == DRIVE_MODE_DEFAULT) && (GV_VCU_HandoverPhase == HO_HOLD))
|
||
{
|
||
nextMode = DRIVE_MODE_DEFAULT;
|
||
nextPhase = HO_NONE;
|
||
}
|
||
// Requirement: Default mode + not stopped + RC_REQ_DEFAULT -> Emergency
|
||
else if (curMode == DRIVE_MODE_ACU && !HasAcuRequest() && !IsVehicleStopped())
|
||
{
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
nextPhase = HO_HOLD;
|
||
GV_VCU_EmergencyCause = EMG_HO_SPD_COND_FAIL;
|
||
}
|
||
else if(curMode == DRIVE_MODE_EMERGENCY)
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
// ACU mode -> Default handover
|
||
else if (curMode == DRIVE_MODE_ACU)
|
||
{
|
||
if (!IsVehicleStopped())
|
||
{
|
||
nextPhase = HO_INPROG;
|
||
}
|
||
else
|
||
{
|
||
nextPhase = HO_PERMIT;
|
||
if (!HasAcuRequest())
|
||
{
|
||
nextMode = DRIVE_MODE_DEFAULT;
|
||
nextPhase = HO_NONE;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (curMode != DRIVE_MODE_RC || IsVehicleStopped())
|
||
{
|
||
nextMode = DRIVE_MODE_DEFAULT;
|
||
nextPhase = HO_NONE;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case RC_REQ_MANUAL:
|
||
if (curMode == DRIVE_MODE_ACU && !IsRcHandoverReady())
|
||
{
|
||
s_acuToRcHandoverActive = true;
|
||
if (HasAcuRequest())
|
||
s_acuToRcSourceSeen = true;
|
||
|
||
nextPhase = HO_INPROG;
|
||
}
|
||
else if (curMode == DRIVE_MODE_ACU)
|
||
{
|
||
nextPhase = HO_PERMIT;
|
||
if (!HasAcuRequest())
|
||
{
|
||
nextMode = DRIVE_MODE_RC;
|
||
nextPhase = HO_NONE;
|
||
s_acuToRcHandoverActive = false;
|
||
s_acuToRcSourceSeen = false;
|
||
}
|
||
}
|
||
else if (curMode == DRIVE_MODE_DEFAULT && IsVehicleStopped())
|
||
{
|
||
nextMode = DRIVE_MODE_RC;
|
||
nextPhase = HO_NONE;
|
||
}
|
||
else if(curMode == DRIVE_MODE_RC)
|
||
{
|
||
nextMode = DRIVE_MODE_RC;
|
||
nextPhase = HO_NONE;
|
||
}
|
||
else
|
||
{
|
||
nextPhase = HO_INPROG;
|
||
}
|
||
break;
|
||
|
||
case RC_REQ_AUTO:
|
||
if (curMode == DRIVE_MODE_ACU && !HasAcuRequest())
|
||
{
|
||
nextPhase = HO_HOLD;
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
GV_VCU_EmergencyCause = EMG_HO_CONFLICT;
|
||
}
|
||
else if ((curMode == DRIVE_MODE_RC) &&
|
||
(GV_RC_Drive_Mode == 0U) &&
|
||
(GV_RC_ACU_Drive_Mode == 1U) &&
|
||
!IsAcuHandoverReady())
|
||
{
|
||
s_rcToAcuHandoverActive = true;
|
||
nextPhase = HO_INPROG;
|
||
}
|
||
else if ((curMode == DRIVE_MODE_RC) &&
|
||
(GV_RC_Drive_Mode == 0U) &&
|
||
(GV_RC_ACU_Drive_Mode == 1U))
|
||
{
|
||
s_rcToAcuHandoverActive = true;
|
||
nextPhase = HO_PERMIT;
|
||
if (HasAcuRequest())
|
||
{
|
||
nextMode = DRIVE_MODE_ACU;
|
||
nextPhase = HO_NONE;
|
||
s_rcToAcuHandoverActive = false;
|
||
s_rcToAcuSourceSeen = false;
|
||
}
|
||
else
|
||
{
|
||
autoWaitOn = true;
|
||
if (Time_GetAutoOnWaitSec() >= 30U)
|
||
{
|
||
nextPhase = HO_HOLD;
|
||
}
|
||
}
|
||
}
|
||
else if ((curMode == DRIVE_MODE_DEFAULT) && (GV_VCU_HandoverPhase == HO_HOLD))
|
||
{
|
||
nextMode = DRIVE_MODE_DEFAULT;
|
||
nextPhase = HO_HOLD;
|
||
}
|
||
else if (curMode == DRIVE_MODE_DEFAULT && nextPhase != HO_HOLD && IsVehicleStopped())
|
||
{
|
||
nextPhase = HO_PERMIT;
|
||
if (HasAcuRequest())
|
||
{
|
||
nextMode = DRIVE_MODE_ACU;
|
||
nextPhase = HO_NONE;
|
||
}
|
||
else
|
||
{
|
||
autoWaitOn = true;
|
||
if (Time_GetAutoOnWaitSec() >= 30U)
|
||
{
|
||
nextPhase = HO_HOLD;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
nextPhase = HO_NONE;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
nextMode = curMode;
|
||
nextPhase = HO_NONE;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (IsRcRequestCompleted(s_rcModeReqRaw, nextMode))
|
||
GV_RC_ModeReq = RC_REQ_NONE;
|
||
else
|
||
GV_RC_ModeReq = s_rcModeReqRaw;
|
||
|
||
Time_SetAutoOnWait(autoWaitOn);
|
||
}
|
||
|
||
|
||
|
||
void RcRequestCheck(void)
|
||
{
|
||
if ((GV_RC_Drive_Mode == 1U) && (s_prevRcManualOn == 0U)) s_lastRcModeReq = RC_REQ_MANUAL;
|
||
if ((GV_RC_ACU_Drive_Mode == 1U) && (s_prevRcAutoOn == 0U)) s_lastRcModeReq = RC_REQ_AUTO;
|
||
|
||
GV_TradeOff_Emergency_Flag = 0;
|
||
|
||
if ((GV_RC_ACU_Drive_Mode == 1U) && (GV_RC_Drive_Mode == 1U))
|
||
{
|
||
if (s_lastRcModeReq == RC_REQ_AUTO) s_rcModeReqRaw = RC_REQ_AUTO;
|
||
else s_rcModeReqRaw = RC_REQ_MANUAL;
|
||
|
||
GV_TradeOff_Emergency_Flag = 1;
|
||
GV_VCU_EmergencyCause = EMG_HO_CONFLICT;
|
||
}
|
||
else if (GV_RC_ACU_Drive_Mode == 1U)
|
||
{
|
||
s_rcModeReqRaw = RC_REQ_AUTO;
|
||
s_lastRcModeReq = RC_REQ_AUTO;
|
||
|
||
GV_TradeOff_Emergency_Flag = 0;
|
||
}
|
||
else if (GV_RC_Drive_Mode == 1U)
|
||
{
|
||
s_rcModeReqRaw = RC_REQ_MANUAL;
|
||
s_lastRcModeReq = RC_REQ_MANUAL;
|
||
|
||
GV_TradeOff_Emergency_Flag = 0;
|
||
}
|
||
else
|
||
{
|
||
if (s_lastRcModeReq == RC_REQ_AUTO)
|
||
s_rcModeReqRaw = RC_REQ_NONE;
|
||
else
|
||
s_rcModeReqRaw = RC_REQ_DEFAULT;
|
||
|
||
s_lastRcModeReq = s_rcModeReqRaw;
|
||
|
||
GV_TradeOff_Emergency_Flag = 0;
|
||
}
|
||
|
||
s_prevRcManualOn = (GV_RC_Drive_Mode == 1U) ? 1U : 0U;
|
||
s_prevRcAutoOn = (GV_RC_ACU_Drive_Mode == 1U) ? 1U : 0U;
|
||
}
|
||
|
||
|
||
|
||
/*============================== Fault Check ==============================*/
|
||
void CheckSystemFault(void)
|
||
{
|
||
GV_BMS_Fault_Check = !(GV_BMS_FltSta == 0 && GV_BMS_TEMP_FAULT == 0 && GV_BMS_Interlock == 1); // BMS Fault Check
|
||
GV_MCU_Fault_Check = !(GV_MCU_FltSta == 0); // MCU Fault Check(0 = Connected)
|
||
GV_LDC_Fault_Check = !(GV_LDC_FltSta == 0 && GV_LDC_TEMP_FAULT == 0); // LDC Fault Check
|
||
GV_OBC_Fault_Check = !(GV_OBC_Interlock == 1); // OBC Fault Check
|
||
GV_HV_Apply_Check = !(GV_BMS_HVOn_STA == 0); // BMS High voltage Status Check
|
||
GV_LDC_Apply_Check = !(GV_LDC_STA == 0); // LDC Status Check
|
||
}
|
||
|
||
|
||
/*============================== Default Mode ==============================*/
|
||
void ExecuteDefaultMode()
|
||
{
|
||
HighVoltageOff();
|
||
DefaultLampSet();
|
||
GV_VCU_RC_Emergency_Stop = 0;
|
||
GV_VCU_ACU_Emergency_Stop = 0;
|
||
}
|
||
|
||
|
||
/*============================== ACU Mode ==============================*/
|
||
void ExecuteACUMode()
|
||
{
|
||
HighVoltageOn();
|
||
ACULampSet();
|
||
|
||
if(GV_ACU_Emergency_Stop != 1) GV_VCU_ACU_Emergency_Stop = 0;
|
||
|
||
ACU_Arc_check();
|
||
}
|
||
|
||
|
||
/*============================== RC Mode ==============================*/
|
||
void ExecuteRCMode()
|
||
{
|
||
HighVoltageOn();
|
||
RCLampSet();
|
||
|
||
if(GV_RC_Emergency_Stop != 1) GV_VCU_RC_Emergency_Stop = 0;
|
||
|
||
GV_VCU_Vx_Limit = GV_RC_VX_Limit;
|
||
}
|
||
|
||
|
||
/*============================== Emergency Mode ==============================*/
|
||
static bool s_emergencyAutoParkPending = true;
|
||
void ExecuteEmergencyMode()
|
||
{
|
||
|
||
if (GV_VCU_Vx_Fbk >= 0.1f) // 1) While vehicle is moving, force N.
|
||
GV_VCU_GearSelStat = 2U; // N
|
||
else if (s_emergencyAutoParkPending)// 2) First stop in emergency: apply P exactly once.
|
||
{
|
||
GV_VCU_GearSelStat = 0U; // P
|
||
s_emergencyAutoParkPending = false;
|
||
}
|
||
else if ((GV_RC_GearSelStat == 0U) || (GV_RC_GearSelStat == 2U)) // 3) After first auto-park, allow RC to toggle only P/N.
|
||
GV_VCU_GearSelStat = GV_RC_GearSelStat;
|
||
else // Default to P if RC command is invalid
|
||
GV_VCU_GearSelStat = 0U;
|
||
|
||
GV_VCU_RC_Emergency_Stop = 1;
|
||
GV_VCU_ACU_Emergency_Stop = 1;
|
||
|
||
// 고전??off
|
||
HighVoltageOff();
|
||
|
||
EmergencyLampSet();
|
||
|
||
// Keep emergency latched for handover speed-fail until RC requests MANUAL.
|
||
if (!isEmergency && GV_VCU_EmergencyCause == EMG_HO_SPD_COND_FAIL && s_rcModeReqRaw != RC_REQ_MANUAL)
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
else if (!isEmergency && GV_VCU_EmergencyCause == EMG_HO_CONFLICT &&
|
||
s_rcModeReqRaw != RC_REQ_DEFAULT && s_rcModeReqRaw != RC_REQ_MANUAL)
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
|
||
// Exit emergency mode when no emergency source remains.
|
||
else if(!isEmergency && !isEmergencyVcu &&
|
||
s_rcModeReqRaw == RC_REQ_MANUAL &&
|
||
IsRcParkRequest())
|
||
nextMode = DRIVE_MODE_RC;
|
||
else if(!isEmergency && !isEmergencyVcu &&
|
||
s_rcModeReqRaw == RC_REQ_DEFAULT &&
|
||
IsRcParkRequest())
|
||
nextMode = DRIVE_MODE_DEFAULT;
|
||
else if (!isEmergency && IsRcParkRequest())
|
||
{
|
||
GV_Lamp_BRAKE_CMD = 0;
|
||
GV_Lamp_Hazard_CMD = 0;
|
||
|
||
GV_VCU_RC_Emergency_Stop = 0;
|
||
GV_VCU_ACU_Emergency_Stop = 0;
|
||
|
||
s_emergencyAutoParkPending = true;
|
||
|
||
nextMode = DRIVE_MODE_DEFAULT;
|
||
nextPhase = HO_NONE;
|
||
}
|
||
else
|
||
nextMode = DRIVE_MODE_EMERGENCY;
|
||
}
|
||
|
||
|
||
/*============================== Longitudinal control mode change ==============================*/
|
||
void LongitudinalCtrlMode(void)
|
||
{
|
||
// RC Mode
|
||
if(GV_VCU_Drive_Mode == 1 && GV_RC_VCU_long_Ctl_mode == 1 && GV_RC_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_RC_Drive_ACC_Cmd == 0)
|
||
GV_VCU_RC_long_Ctl_mode = 1;
|
||
else if(GV_VCU_Drive_Mode == 1 && GV_RC_VCU_long_Ctl_mode == 0 && GV_RC_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_RC_Drive_ACC_Cmd == 0)
|
||
GV_VCU_RC_long_Ctl_mode = 0;
|
||
|
||
// ACU Mode
|
||
if(GV_VCU_Drive_Mode == 0 && GV_ACU_long_Ctl_mode == 1 && GV_ACU_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_ACU_Drive_ACC_Cmd == 0)
|
||
GV_VCU_ACU_long_Ctl_mode = 1;
|
||
else if(GV_VCU_Drive_Mode == 0 && GV_ACU_long_Ctl_mode == 0 && GV_ACU_Vx_Command == 0 && GV_VCU_Vx_Fbk <= 1 && GV_ACU_Drive_ACC_Cmd == 0)
|
||
GV_VCU_ACU_long_Ctl_mode = 0;
|
||
|
||
GV_VCU_AccControlFlag = GV_VCU_RC_long_Ctl_mode || GV_VCU_ACU_long_Ctl_mode;
|
||
}
|
||
|
||
|
||
/*============================== Set Vehicle Environment ==============================*/
|
||
void SetVehicleEnvironment()
|
||
{
|
||
//----- Wake-up -----//
|
||
GV_IDB_032_WakeUpCommand = 1;
|
||
GV_RCU_033_WakeUpCommand = 1;
|
||
|
||
GV_RWA_310_ModeCommand = 1;
|
||
GV_RWA_310_WakeupCommand = 1;
|
||
GV_RWA2_310_ModeCommand = 0;
|
||
GV_RWA2_310_WakeupCommand = 0;
|
||
|
||
GV_RWS1_310_WakeupCommand = 1;
|
||
GV_RWS1_310_ModeCommand = 1;
|
||
GV_RWS2_310_WakeupCommand = 0;
|
||
GV_RWS2_310_ModeCommand = 0;
|
||
|
||
|
||
//----- Command -----//
|
||
GV_DriveTorqueCommand = GV_Motor_Torque_Cmd;
|
||
GV_DriveTorqueCommandValid = 1;
|
||
GV_PowertrainMode = 1;
|
||
//GV_GearPosition = GV_VCU_GearSelStat;
|
||
GV_RCU_PowertrainMode = 1;
|
||
GV_RCU_GearPosition = GV_VCU_GearSelStat;
|
||
|
||
GV_RWA_040_RackAngleCmdValid = 1;
|
||
GV_RWA2_040_RackAngleCmdValid = 1;
|
||
|
||
GV_RWS1_040_RackAngleCmdValid = 1;
|
||
GV_RWS2_040_RackAngleCmdValid = 1;
|
||
|
||
|
||
GV_Env_384_EnvironmentTemperature = 0;
|
||
GV_Env_384_EnvironmentTemperatureValid = 0;
|
||
GV_Env_384_TimestampDay = 21;
|
||
GV_Env_384_TimestampHour = 0;
|
||
GV_Env_384_TimestampMinute = 0;
|
||
GV_Env_384_TimestampMonth = 6;
|
||
GV_Env_384_TimestampSecond = 0;
|
||
GV_Env_384_TimestampYear = 24;
|
||
}
|
||
|
||
|
||
/*============================== High voltage on ==============================*/
|
||
void HighVoltageOn(void)
|
||
{
|
||
/*Check Fault Status, Contact HV Sys*/
|
||
GV_HV_ON_CMD = 1;
|
||
|
||
/*Check Fault Status, Contact LDC Sys*/
|
||
if(GV_HV_Apply_Check == 1)
|
||
{
|
||
GV_LDC_ACT_CMD = 1;
|
||
GV_LDC_TAGT_CMD = 120; // 20240821- 30->40?<3F>로 변<>? 추후 ?<3F>율주행 ??180 ?<3F>로 조정 // 20241010 - 150A <20>?조정// 20241024-> ?<3F>이?<3F>플??<3F><> ?<3F>계 120A <20>?조정
|
||
}
|
||
}
|
||
|
||
|
||
/*============================== High voltage off ==============================*/
|
||
void HighVoltageOff(void)
|
||
{
|
||
if (GV_LDC_Apply_Check == 0 && GV_HV_ON_CMD == 1)
|
||
CNT_GV_HV_ON_CMD++;
|
||
|
||
if (CNT_GV_HV_ON_CMD >= 500) // Wait for 1000ms
|
||
{
|
||
CNT_GV_HV_ON_CMD = 0;
|
||
GV_HV_ON_CMD = 0;
|
||
GV_IG_Process_Status = KEY_STATE_OFF_PROCESS_WAIT_LDC;
|
||
}
|
||
|
||
GV_LDC_ACT_CMD = 0;
|
||
GV_LDC_TAGT_CMD = 0;
|
||
}
|
||
|