|
@@ -81,82 +81,121 @@ void MC_Fault_SpeedSensor_Process(TrueOrFalse_Flag_Struct_t IsStopFlag, MC_Caden
|
|
|
}
|
|
|
|
|
|
//力矩传感器故障检测
|
|
|
-void MC_Fault_TorqueSensor_Process(uint16_t ADC_Data, MC_GearSt_Struct_t GearSt, MC_CadenceResult_Struct_t CadenceData, MC_AssistRunMode_Struct_t AstMode, MC_ErrorCode_Struct_t* p_MC_ErrorCode)
|
|
|
+typedef struct
|
|
|
+{
|
|
|
+ uint32_t TrigTimeCnt_1;
|
|
|
+ uint32_t TrigTimeCnt_2;
|
|
|
+ uint16_t TorqueArray[25];
|
|
|
+ uint16_t FaultType;
|
|
|
+ uint32_t DiffStdResult;
|
|
|
+}ToruqeSensorFaultCheck_Struct_t;
|
|
|
+
|
|
|
+//单个力矩传感器故障检测
|
|
|
+void MC_Fault_TorqueSensor_Process_Single(uint16_t ADC_Data, uint8_t Num, ToruqeSensorFaultCheck_Struct_t* p_SensorCheck, uint8_t Cadence,
|
|
|
+ TorsueSensorFaultStatus_Struct_t* SensorFaultStatus)
|
|
|
{
|
|
|
- static uint32_t TrigTimeCnt_1 = 0;
|
|
|
- static uint32_t TrigTimeCnt_2 = 0;
|
|
|
- static uint16_t TorqueArray[25] = {0};
|
|
|
- static uint32_t DiffSqrtResult = 0;
|
|
|
uint16_t i;
|
|
|
uint16_t DelayTime;
|
|
|
- static uint32_t PeriodTimeCnt = 0;
|
|
|
+
|
|
|
+ for(i=0; i<(sizeof(p_SensorCheck->TorqueArray) / 2 - 1); i++)
|
|
|
+ {
|
|
|
+ p_SensorCheck->TorqueArray[i] = p_SensorCheck->TorqueArray[i + 1];
|
|
|
+ }
|
|
|
+ p_SensorCheck->TorqueArray[sizeof(p_SensorCheck->TorqueArray) / 2 - 1] = ADC_Data;
|
|
|
+
|
|
|
+ p_SensorCheck->DiffStdResult = GetStandardDeviation(p_SensorCheck->TorqueArray, sizeof(p_SensorCheck->TorqueArray) / 2);
|
|
|
+ //标准差较小
|
|
|
+ if(p_SensorCheck->DiffStdResult < 12)
|
|
|
+ {
|
|
|
+ DelayTime = 15000 / Cadence; //根据踏频计算踩踏1/4圈的时间,要考虑空踩情况
|
|
|
+ DelayTime = (DelayTime < 250) ? 250 : DelayTime;
|
|
|
+ if((HAL_GetTick() - p_SensorCheck->TrigTimeCnt_1) > (DelayTime * 2))
|
|
|
+ {
|
|
|
+ SensorFaultStatus->FaultStatusCode |= 1 << (Num - 1);
|
|
|
+ p_SensorCheck->FaultType = 1;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ p_SensorCheck->TrigTimeCnt_1 = HAL_GetTick();
|
|
|
+ }
|
|
|
|
|
|
- if(HAL_GetTick() < 5000)
|
|
|
+ //传感器短路或开路检测
|
|
|
+ if((ADC_Data < 50) || (ADC_Data > 4050))
|
|
|
{
|
|
|
- TrigTimeCnt_1 = HAL_GetTick();
|
|
|
- TrigTimeCnt_2 = HAL_GetTick();
|
|
|
+ if((HAL_GetTick() - p_SensorCheck->TrigTimeCnt_2) > 250)
|
|
|
+ {
|
|
|
+ SensorFaultStatus->FaultStatusCode |= 1 << (Num - 1);
|
|
|
+ p_SensorCheck->FaultType = 2;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ p_SensorCheck->TrigTimeCnt_2 = HAL_GetTick();
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+//力矩传感器故障综合检测
|
|
|
+void MC_Fault_TorqueSensor_Process(uint16_t* SensorData,
|
|
|
+ MC_GearSt_Struct_t GearSt, MC_CadenceResult_Struct_t CadenceData, MC_AssistRunMode_Struct_t AstMode,
|
|
|
+ TorsueSensorFaultStatus_Struct_t* SensorFaultStatus, MC_ErrorCode_Struct_t* p_MC_ErrorCode)
|
|
|
+{
|
|
|
+ static uint32_t PeriodTimeCnt = 0;
|
|
|
+
|
|
|
+ static ToruqeSensorFaultCheck_Struct_t ToruqeSensorFaultCheck_Struct_t[3]; //用于传感器1\2\3的判断
|
|
|
+
|
|
|
+ if((HAL_GetTick() < 5000) || (MC_WorkMode == MC_WorkMode_Config))
|
|
|
+ {
|
|
|
+ ToruqeSensorFaultCheck_Struct_t[0].TrigTimeCnt_1 = HAL_GetTick();
|
|
|
+ ToruqeSensorFaultCheck_Struct_t[0].TrigTimeCnt_2 = HAL_GetTick();
|
|
|
+ ToruqeSensorFaultCheck_Struct_t[1].TrigTimeCnt_1 = HAL_GetTick();
|
|
|
+ ToruqeSensorFaultCheck_Struct_t[1].TrigTimeCnt_2 = HAL_GetTick();
|
|
|
+ ToruqeSensorFaultCheck_Struct_t[2].TrigTimeCnt_1 = HAL_GetTick();
|
|
|
+ ToruqeSensorFaultCheck_Struct_t[2].TrigTimeCnt_2 = HAL_GetTick();
|
|
|
}
|
|
|
|
|
|
- if( MC_WorkMode == MC_WorkMode_Config) return;
|
|
|
//运行周期20ms
|
|
|
if((HAL_GetTick() - PeriodTimeCnt) >= 20)
|
|
|
{
|
|
|
- PeriodTimeCnt = HAL_GetTick();
|
|
|
- for(i=0; i<(sizeof(TorqueArray) / 2 - 1); i++)
|
|
|
+ PeriodTimeCnt = HAL_GetTick();
|
|
|
+
|
|
|
+ //力矩助力模式、踏频正转且大于30rpm
|
|
|
+ if(((GearSt != 0x22) && ((GearSt & 0x0F) != 0)) && (CadenceData.Cadence_Data > 30) && (CadenceData.Cadence_Dir == MC_Cadence_Forward) && (AstMode == MC_AssistRunMode_TORQUE))
|
|
|
{
|
|
|
- TorqueArray[i] = TorqueArray[i + 1];
|
|
|
- }
|
|
|
- TorqueArray[sizeof(TorqueArray) / 2 - 1] = ADC_Data;
|
|
|
+ //判断传感器1
|
|
|
+ MC_Fault_TorqueSensor_Process_Single(SensorData[0], 1, &ToruqeSensorFaultCheck_Struct_t[0], CadenceData.Cadence_Data, SensorFaultStatus);
|
|
|
+
|
|
|
+ //判断传感器2
|
|
|
+ MC_Fault_TorqueSensor_Process_Single(SensorData[1], 2, &ToruqeSensorFaultCheck_Struct_t[1], CadenceData.Cadence_Data, SensorFaultStatus);
|
|
|
|
|
|
- if(p_MC_ErrorCode->ERROR_Bit.Fault_TorqueSensor == 0)
|
|
|
+ //判断传感器3
|
|
|
+ MC_Fault_TorqueSensor_Process_Single(SensorData[2], 3, &ToruqeSensorFaultCheck_Struct_t[2], CadenceData.Cadence_Data, SensorFaultStatus);
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //综合判断
|
|
|
+ if(p_MC_ErrorCode->ERROR_Bit.Fault_TorqueSensor == 0)
|
|
|
+ {
|
|
|
+ if((SensorFaultStatus->FaultBit.Sensor1 + SensorFaultStatus->FaultBit.Sensor2 + SensorFaultStatus->FaultBit.Sensor3) > 1)//超过2个传感器异常
|
|
|
{
|
|
|
- DiffSqrtResult = GetStandardDeviation(TorqueArray, sizeof(TorqueArray) / 2);
|
|
|
- //传感器感应失效检测,进入力矩助力模式、踏频正转大于10rpm、标准差较小
|
|
|
- if(((GearSt != 0x22) && ((GearSt & 0x0F) != 0)) && (CadenceData.Cadence_Data > 30) && (CadenceData.Cadence_Dir == MC_Cadence_Forward) && (AstMode == MC_AssistRunMode_TORQUE) && (DiffSqrtResult < 10))
|
|
|
- {
|
|
|
- DelayTime = 15000 / CadenceData.Cadence_Data; //根据踏频计算踩踏1/4圈的时间,要考虑空踩情况
|
|
|
- DelayTime = (DelayTime < 250) ? 250 : DelayTime;
|
|
|
- if((HAL_GetTick() - TrigTimeCnt_1) > (DelayTime * 2))
|
|
|
- {
|
|
|
- p_MC_ErrorCode->ERROR_Bit.Fault_TorqueSensor = 1;
|
|
|
- //记录故障日志
|
|
|
- MC_ErrorLogSaveInfo.NotesInfo1 = 1;
|
|
|
- MC_ErrorLogSaveInfo.NotesInfo2 = (uint16_t)(DiffSqrtResult);
|
|
|
- ErrorLogSave_Update(&MC_ErrorLogSaveInfo);
|
|
|
- IsErrorLogSaveInfoUpdateFlag = TRUE;
|
|
|
- //存储故障次数
|
|
|
- MC_RunLog1.TQS_FaultCnt++;
|
|
|
- RunLogSaveIndex = 1;
|
|
|
-
|
|
|
- return;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- TrigTimeCnt_1 = HAL_GetTick();
|
|
|
- }
|
|
|
- //传感器短路或开路检测
|
|
|
- if((ADC_Data < 50) || (ADC_Data > 4050))
|
|
|
- {
|
|
|
- if((HAL_GetTick() - TrigTimeCnt_2) > 200)
|
|
|
- {
|
|
|
- p_MC_ErrorCode->ERROR_Bit.Fault_TorqueSensor = 1;
|
|
|
- //记录故障日志
|
|
|
- MC_ErrorLogSaveInfo.NotesInfo1 = 2;
|
|
|
- MC_ErrorLogSaveInfo.NotesInfo2 = ADC_Data;
|
|
|
- ErrorLogSave_Update(&MC_ErrorLogSaveInfo);
|
|
|
- IsErrorLogSaveInfoUpdateFlag = TRUE;
|
|
|
- //存储故障次数
|
|
|
- MC_RunLog1.TQS_FaultCnt++;
|
|
|
- RunLogSaveIndex = 1;
|
|
|
-
|
|
|
- return;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- TrigTimeCnt_2 = HAL_GetTick();
|
|
|
- }
|
|
|
+ p_MC_ErrorCode->ERROR_Bit.Fault_TorqueSensor = 1;
|
|
|
+ //记录故障日志
|
|
|
+ MC_ErrorLogSaveInfo.NotesInfo1 = (SensorFaultStatus->FaultStatusCode << 8); //高8位存储3个传感器的故障状态
|
|
|
+ MC_ErrorLogSaveInfo.NotesInfo1 |= (ToruqeSensorFaultCheck_Struct_t[0].FaultType) +
|
|
|
+ (ToruqeSensorFaultCheck_Struct_t[1].FaultType << 2) +
|
|
|
+ (ToruqeSensorFaultCheck_Struct_t[2].FaultType << 4); //依次存储3个传感器的故障类型
|
|
|
+ MC_ErrorLogSaveInfo.NotesInfo2 = (ToruqeSensorFaultCheck_Struct_t[0].DiffStdResult) + //依次存储传感器1、2的标准差
|
|
|
+ (ToruqeSensorFaultCheck_Struct_t[1].DiffStdResult << 8);
|
|
|
+ MC_ErrorLogSaveInfo.NotesInfo3 = (ToruqeSensorFaultCheck_Struct_t[2].DiffStdResult); //存储传感器3的标准差
|
|
|
+ ErrorLogSave_Update(&MC_ErrorLogSaveInfo);
|
|
|
+ IsErrorLogSaveInfoUpdateFlag = TRUE;
|
|
|
+ //存储故障次数
|
|
|
+ MC_RunLog1.TQS_FaultCnt++;
|
|
|
+ RunLogSaveIndex = 1;
|
|
|
+ return;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
@@ -994,7 +1033,7 @@ void MC_Fault_Check_Process(void)
|
|
|
}
|
|
|
|
|
|
//力矩传感器故障检测
|
|
|
- MC_Fault_TorqueSensor_Process(ADC1_Result[ADC1_RANK_TORQUE_SENSOR_1], MC_ControlCode.GearSt, MC_CadenceResult, MC_CalParam.AssistRunMode, &MC_ErrorCode);
|
|
|
+ MC_Fault_TorqueSensor_Process(&ADC1_Result[ADC1_RANK_TORQUE_SENSOR_1], MC_ControlCode.GearSt, MC_CadenceResult, MC_CalParam.AssistRunMode, &TorsueSensorFaultStatus, &MC_ErrorCode);
|
|
|
|
|
|
//相线故障检测
|
|
|
MC_Fault_PhaseLine_Process(MC_CalParam.Foc_Flag, MC_RunInfo.BusCurrent, MC_RunInfo.MotorSpeed, ADC_3ShuntCurrent_RMSValue, &MC_ErrorCode);
|