/** ****************************************************************************** * @file cp_detection.c * @author Edward Lien * @brief This file provides Control Pilot Voltage detection with filter algorithm. * @Version D0.03 * @Date 2020/03/04 ****************************************************************************** */ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "main.h" #include "cp_detection.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ #ifdef FUNC_CP_ADC_MODIFY #ifdef NEW_CP_SPEC sCPVoltageBoundary_t CpModuleBoundary = { 0 }; sCPVoltageBoundary_t SpecBoundary = { 0 }; sCPVoltageHysteresis_t CpModuleHysteresis = { 0 }; #ifdef NEW_CP_SPEC_20210525 //--------------------------------------------------------[ GEN ] sCPVoltageBoundary_t CpModuleBoundary_20210525_GEN = { CP_GET_OPA_VOUT_ADC(12 + 1.5), CP_GET_OPA_VOUT_ADC(12 - 1.5), CP_GET_OPA_VOUT_ADC(9 + 1.5), CP_GET_OPA_VOUT_ADC(9 - 1.5), CP_GET_OPA_VOUT_ADC(6 + 1.5), CP_GET_OPA_VOUT_ADC(6 - 1.5), CP_GET_OPA_VOUT_ADC(3 + 1.5), CP_GET_OPA_VOUT_ADC(3 - 1.5), CP_GET_OPA_VOUT_ADC(0 + 1.5), CP_GET_OPA_VOUT_ADC(0 - 1.5), CP_GET_OPA_VOUT_ADC(-12 + 2.5), CP_GET_OPA_VOUT_ADC(-12 - 2.5) }; sCPVoltageBoundary_t SpecBoundary_20210525_GEN = { CP_GET_OPA_VOUT_ADC(12 + 1.5), CP_GET_OPA_VOUT_ADC(12 - 1.5), CP_GET_OPA_VOUT_ADC(9 + 1.5), CP_GET_OPA_VOUT_ADC(9 - 1.5), CP_GET_OPA_VOUT_ADC(6 + 1.5), CP_GET_OPA_VOUT_ADC(6 - 1.5), CP_GET_OPA_VOUT_ADC(3 + 1.5), CP_GET_OPA_VOUT_ADC(3 - 1.5), CP_GET_OPA_VOUT_ADC(0 + 1.5), CP_GET_OPA_VOUT_ADC(0 - 1.5), CP_GET_OPA_VOUT_ADC(-12 + 2.5), CP_GET_OPA_VOUT_ADC(-12 - 2.5) }; sCPVoltageHysteresis_t CpModuleHysteresis_20210525_GEN = { CP_GET_OPA_VOLT_DIFF_ADC(0.5), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.5), CP_GET_OPA_VOLT_DIFF_ADC(0.5), CP_GET_OPA_VOLT_DIFF_ADC(0.5) }; //--------------------------------------------------------[ GB ] sCPVoltageBoundary_t CpModuleBoundary_20210525_GB = { CP_GET_OPA_VOUT_ADC(12 + 1.5), CP_GET_OPA_VOUT_ADC(12 - 1.5), CP_GET_OPA_VOUT_ADC(9 + 1.5), CP_GET_OPA_VOUT_ADC(9 - 1.5), CP_GET_OPA_VOUT_ADC(6 + 1.5), CP_GET_OPA_VOUT_ADC(6 - 1.5), CP_GET_OPA_VOUT_ADC(3 + 1.5), CP_GET_OPA_VOUT_ADC(3 - 1.5), CP_GET_OPA_VOUT_ADC(0 + 1.5), CP_GET_OPA_VOUT_ADC(0 - 1.5), CP_GET_OPA_VOUT_ADC(-12 + 2.5), CP_GET_OPA_VOUT_ADC(-12 - 2.5) }; //sCPVoltageBoundary_t SpecBoundary_20210525_GB = { // CP_GET_OPA_VOUT_ADC(12 + 0.8), // CP_GET_OPA_VOUT_ADC(12 - 0.8), // CP_GET_OPA_VOUT_ADC(9 + 0.8), // CP_GET_OPA_VOUT_ADC(9 - 0.8), // CP_GET_OPA_VOUT_ADC(6 + 0.8), // CP_GET_OPA_VOUT_ADC(6 - 0.8), // CP_GET_OPA_VOUT_ADC(3 + 0.8), // CP_GET_OPA_VOUT_ADC(3 - 0.8), // CP_GET_OPA_VOUT_ADC(0 + 0.8), // CP_GET_OPA_VOUT_ADC(0 - 0.8), // CP_GET_OPA_VOUT_ADC(-12 + 0.8), // //CP_GET_OPA_VOUT_ADC(-12 - 0.8) // CP_GET_OPA_VOUT_ADC(-12 - 2.5) // }; sCPVoltageBoundary_t SpecBoundary_20210525_GB = { CP_GET_OPA_VOUT_ADC(12 + 1.5), CP_GET_OPA_VOUT_ADC(12 - 1.5), CP_GET_OPA_VOUT_ADC(9 + 1.5), CP_GET_OPA_VOUT_ADC(9 - 1.5), CP_GET_OPA_VOUT_ADC(6 + 1.5), CP_GET_OPA_VOUT_ADC(6 - 1.5), CP_GET_OPA_VOUT_ADC(3 + 1.5), CP_GET_OPA_VOUT_ADC(3 - 1.5), CP_GET_OPA_VOUT_ADC(0 + 1.5), CP_GET_OPA_VOUT_ADC(0 - 1.5), CP_GET_OPA_VOUT_ADC(-12 + 2.5), CP_GET_OPA_VOUT_ADC(-12 - 2.5) }; sCPVoltageHysteresis_t CpModuleHysteresis_20210525_GB = { CP_GET_OPA_VOLT_DIFF_ADC(0.5), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.25), CP_GET_OPA_VOLT_DIFF_ADC(0.5), CP_GET_OPA_VOLT_DIFF_ADC(0.5), CP_GET_OPA_VOLT_DIFF_ADC(0.5) }; #endif //NEW_CP_SPEC_20210525 #endif //NEW_CP_SPEC #else //FUNC_CP_ADC_MODIFY sCPVoltageBoundary_t CpModuleBoundary; sCPVoltageBoundary_t SpecBoundary; sCPVoltageHysteresis_t CpModuleHysteresis; #endif //FUNC_CP_ADC_MODIFY uint16_t CPTempBuffer[MAX_CLASSIFY_SECTION][MAX_SECTION_COUNT]; //PUDTB PTB1 PTB2 PTB3 PTB4 PTB5 NTB1 NUDTB uint8_t CpNewStateCounter=0; uint8_t CpPreviousState; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ /* USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ #ifdef FUNC_CP_ADC_MODIFY #ifdef FUNC_CP_ADC_MODIFY_DEBUG void CP_ADC_OPA_PrintParam(void) { const char *s = "************************"; HTK_UNUSED(s); DEBUG_INFO("%s\r\n", s); DEBUG_INFO("CP_ADC_REFVOLT = %f\r\n", CP_ADC_REFVOLT); DEBUG_INFO("CP_ADC_MIN_COUNT = %d\r\n", CP_ADC_MIN_COUNT); DEBUG_INFO("CP_ADC_MAX_COUNT = %d\r\n", CP_ADC_MAX_COUNT); DEBUG_INFO("CP_ADC_RESOLUTION = %f\r\n", CP_ADC_RESOLUTION); DEBUG_INFO("%s\r\n", s); DEBUG_INFO("CP_ADC_OPA_RIN_P = %f\r\n", CP_ADC_OPA_RIN_P); DEBUG_INFO("CP_ADC_OPA_RIN_N = %f\r\n", CP_ADC_OPA_RIN_N); DEBUG_INFO("CP_ADC_OPA_RG = %f\r\n", CP_ADC_OPA_RG); DEBUG_INFO("CP_ADC_OPA_RF = %f\r\n", CP_ADC_OPA_RF); DEBUG_INFO("CP_ADC_OPA_VIN_P = %f\r\n", CP_ADC_OPA_VIN_P); DEBUG_INFO("CP_ADC_OPA_VOP_P = %f\r\n", CP_ADC_OPA_VOP_P); DEBUG_INFO("CP_ADC_OPA_GAIN = %f\r\n", CP_ADC_OPA_GAIN); DEBUG_INFO("CP_ADC_OPA_GAIN_RECIPROCAL = %f\r\n", CP_ADC_OPA_GAIN_RECIPROCAL); DEBUG_INFO("CP_ADC_OPA_CALC_OPA_VOUT_SLOP = %f\r\n", CP_ADC_OPA_CALC_OPA_VOUT_SLOP); DEBUG_INFO("CP_ADC_OPA_CALC_OPA_VOUT_OFFS = %f\r\n", CP_ADC_OPA_CALC_OPA_VOUT_OFFS); DEBUG_INFO("CP_ADC_OPA_CALC_OPA_VIN_SLOP = %f\r\n", CP_ADC_OPA_CALC_OPA_VIN_SLOP); DEBUG_INFO("CP_ADC_OPA_CALC_OPA_VIN_OFFS = %f\r\n", CP_ADC_OPA_CALC_OPA_VIN_OFFS); DEBUG_INFO("%s\r\n", s); DEBUG_INFO("CP_P12V_HIGH = %d\r\n", CpModuleBoundary.CpP12VHigh); DEBUG_INFO("CP_P12V_LOW = %d\r\n", CpModuleBoundary.CpP12VLow); DEBUG_INFO("CP_P9V_HIGH = %d\r\n", CpModuleBoundary.CpP9VHigh); DEBUG_INFO("CP_P9V_LOW = %d\r\n", CpModuleBoundary.CpP9VLow); DEBUG_INFO("CP_P6V_HIGH = %d\r\n", CpModuleBoundary.CpP6VHigh); DEBUG_INFO("CP_P6V_LOW = %d\r\n", CpModuleBoundary.CpP6VLow); DEBUG_INFO("CP_P3V_HIGH = %d\r\n", CpModuleBoundary.CpP3VHigh); DEBUG_INFO("CP_P3V_LOW = %d\r\n", CpModuleBoundary.CpP3VLow); DEBUG_INFO("CP_0V_HIGH = %d\r\n", CpModuleBoundary.Cp0VHigh); DEBUG_INFO("CP_0V_LOW = %d\r\n", CpModuleBoundary.Cp0VLow); DEBUG_INFO("CP_N12V_HIGH = %d\r\n", CpModuleBoundary.CpN12VHigh); DEBUG_INFO("CP_N12V_LOW = %d\r\n", CpModuleBoundary.CpN12VLow); DEBUG_INFO("%s\r\n", s); DEBUG_INFO("CP_SPEC_P12V_HIGH = %d\r\n", SpecBoundary.CpP12VHigh); DEBUG_INFO("CP_SPEC_P12V_LOW = %d\r\n", SpecBoundary.CpP12VLow); DEBUG_INFO("CP_SPEC_P9V_HIGH = %d\r\n", SpecBoundary.CpP9VHigh); DEBUG_INFO("CP_SPEC_P9V_LOW = %d\r\n", SpecBoundary.CpP9VLow); DEBUG_INFO("CP_SPEC_P6V_HIGH = %d\r\n", SpecBoundary.CpP6VHigh); DEBUG_INFO("CP_SPEC_P6V_LOW = %d\r\n", SpecBoundary.CpP6VLow); DEBUG_INFO("CP_SPEC_P3V_HIGH = %d\r\n", SpecBoundary.CpP3VHigh); DEBUG_INFO("CP_SPEC_P3V_LOW = %d\r\n", SpecBoundary.CpP3VLow); DEBUG_INFO("CP_SPEC_0V_HIGH = %d\r\n", SpecBoundary.Cp0VHigh); DEBUG_INFO("CP_SPEC_0V_LOW = %d\r\n", SpecBoundary.Cp0VLow); DEBUG_INFO("CP_SPEC_N12V_HIGH = %d\r\n", SpecBoundary.CpN12VHigh); DEBUG_INFO("CP_SPEC_N12V_LOW = %d\r\n", SpecBoundary.CpN12VLow); DEBUG_INFO("%s\r\n", s); DEBUG_INFO("HYSTERESIS_P12V_HIGH = %d\r\n", CpModuleHysteresis.HysteresisP12VHigh); DEBUG_INFO("HYSTERESIS_P12V_LOW = %d\r\n", CpModuleHysteresis.HysteresisP12VLow); DEBUG_INFO("HYSTERESIS_P9V_HIGH = %d\r\n", CpModuleHysteresis.HysteresisP9VHigh); DEBUG_INFO("HYSTERESIS_P9V_LOW = %d\r\n", CpModuleHysteresis.HysteresisP9VLow); DEBUG_INFO("HYSTERESIS_P6V_HIGH = %d\r\n", CpModuleHysteresis.HysteresisP6VHigh); DEBUG_INFO("HYSTERESIS_P6V_LOW = %d\r\n", CpModuleHysteresis.HysteresisP6VLow); DEBUG_INFO("HYSTERESIS_P3V_HIGH = %d\r\n", CpModuleHysteresis.HysteresisP3VHigh); DEBUG_INFO("HYSTERESIS_P3V_LOW = %d\r\n", CpModuleHysteresis.HysteresisP3VLow); DEBUG_INFO("HYSTERESIS_0V_HIGH = %d\r\n", CpModuleHysteresis.Hysteresis0VHigh); DEBUG_INFO("HYSTERESIS_0V_LOW = %d\r\n", CpModuleHysteresis.Hysteresis0VLow); DEBUG_INFO("HYSTERESIS_N12V_HIGH = %d\r\n", CpModuleHysteresis.HysteresisN12VHigh); DEBUG_INFO("HYSTERESIS_N12V_LOW = %d\r\n", CpModuleHysteresis.HysteresisN12VLow); DEBUG_INFO("%s\r\n", s); } void CP_ADC_OPA_TestFormula(void) { enum { ITEM_NUM = 6 }; const float Item_Vin[ITEM_NUM] = { 12.0, 9.0, 6.0, 3.0, 0, -12.0}; float Item_Vout[ITEM_NUM] = { 0 }; const char *s = "************************"; HTK_UNUSED(Item_Vout); HTK_UNUSED(s); DEBUG_INFO("%s\r\n", s); DEBUG_INFO("%s\r\n", "CP_ADC_OPA_TestFormula"); DEBUG_INFO("%s\r\n", s); for (uint8_t i = 0; i < ITEM_NUM; i++) { Item_Vout[i] = CP_GET_OPA_VOUT(Item_Vin[i]); DEBUG_INFO("CP_GET_OPA_VOUT(%f) = %f\r\n", Item_Vin[i], Item_Vout[i]); } DEBUG_INFO("%s\r\n", s); for (uint8_t i = 0; i < ITEM_NUM; i++) { DEBUG_INFO("CP_GET_OPA_VIN(%f) = %f\r\n", Item_Vout[i], CP_GET_OPA_VIN(Item_Vout[i])); } DEBUG_INFO("%s\r\n", s); } void CP_ADC_OPA_TestBoundaryValue(void) { enum { ITEM_NUM = 12 + 12 + 10 }; const float Item_Vin[ITEM_NUM] = { //CP_P?V_HIGH/LOW 12.0 + 1.5, 12.0 - 1.5, 9.0 + 1.5, 9.0 - 1.5, 6.0 + 1.5, 6.0 - 1.5, 3.0 + 1.5, 3.0 - 1.5, 0.0 + 1.5, 0.0 - 1.5, -12.0 + 1.5, -12.0 - 1.5, //CP_SPEC_P?V_HIGH/LOW 12.0 + 1.0, 12.0 - 1.0, 9.0 + 1.0, 9.0 - 1.0, 6.0 + 1.0, 6.0 - 1.0, 3.0 + 1.0, 3.0 - 1.0, 0.0 + 1.0, 0.0 - 1.0, -12.0 + 1.0, -12.0 - 1.0, //HYSTERESIS_P?V_HIGH/LOW 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, }; const char *s = "************************"; HTK_UNUSED(Item_Vin); HTK_UNUSED(s); DEBUG_INFO("%s\r\n", s); for (uint8_t i = 0; i < ITEM_NUM; i++) { if (i < 24) DEBUG_INFO("CP_GET_OPA_VOUT_ADC(%.1f) = %d\r\n", Item_Vin[i], CP_GET_OPA_VOUT_ADC(Item_Vin[i])); else DEBUG_INFO("CP_GET_OPA_VOLT_DIFF_ADC(%.1f) = %d\r\n", Item_Vin[i], CP_GET_OPA_VOLT_DIFF_ADC(Item_Vin[i])); } DEBUG_INFO("%s\r\n", s); } #endif //FUNC_CP_ADC_MODIFY_DEBUG #ifdef NEW_CP_SPEC void CpDetectModuleInitialize(char SafetyRegulationCode) { #ifdef NEW_CP_SPEC_20210525 if (SafetyRegulationCode == 'G') //GB { memcpy(&CpModuleBoundary, &CpModuleBoundary_20210525_GB, sizeof(sCPVoltageBoundary_t)); memcpy(&SpecBoundary, &SpecBoundary_20210525_GB, sizeof(sCPVoltageBoundary_t)); memcpy(&CpModuleHysteresis, &CpModuleHysteresis_20210525_GB, sizeof(sCPVoltageHysteresis_t)); } else { memcpy(&CpModuleBoundary, &CpModuleBoundary_20210525_GEN, sizeof(sCPVoltageBoundary_t)); memcpy(&SpecBoundary, &SpecBoundary_20210525_GEN, sizeof(sCPVoltageBoundary_t)); memcpy(&CpModuleHysteresis, &CpModuleHysteresis_20210525_GEN, sizeof(sCPVoltageHysteresis_t)); } #endif //NEW_CP_SPEC_20210525 CpNewStateCounter = 0; CpPreviousState = SYSTEM_STATE_A; #ifdef FUNC_CP_ADC_MODIFY_DEBUG CP_ADC_OPA_PrintParam(); CP_ADC_OPA_TestFormula(); CP_ADC_OPA_TestBoundaryValue(); #endif } #endif //NEW_CP_SPEC #endif //FUNC_CP_ADC_MODIFY #ifdef TRACE_CP void TRACE_CP_Print_CPModuleResult(sCPModuleResult_t *p, uint8_t bOK, long EQ_Sum, long EQ_NG1, long EQ_NG2) { XP("%4d %4d %4d %4d %4d %4d %4d (%s) [%7d, %7d, %7d, %7.3f%%]\r\n", p->State, p->PositiveValue, p->NegativeValue, p->MaxPTB, p->MaxPTBCount, p->MaxNTB, p->MaxNTBCount, bOK ? "OK" : "NG", EQ_Sum, EQ_NG1, EQ_NG2, (((float)EQ_NG1 + EQ_NG2) / EQ_Sum) * 100 ); } #endif void CpCalculate(uint32_t * adc_value, uint16_t sample_times, sCPModuleResult_t *result) { uint16_t i,j; uint16_t ClassifyIndex[MAX_CLASSIFY_SECTION],ClassifyIndexMax = 0,MaxSection = 0; uint64_t PositiveAdcSectionSum = 0, PositiveAdcSectionAvg = 0, NegativeAdcSectionSum = 0, NegativeAdcSectionAvg = 0; uint8_t CpNewState,IndexCounterEqual=false,FlagStateUnknown=false; #ifdef FUNC_CP_VARIABLE_MAX_STATE_COUNTER uint8_t MaxStateCounter = MAX_STATE_COUNTER; #endif for(i=0;iCpModuleBoundary.CpP12VHigh)){ CPTempBuffer[1][ClassifyIndex[1]] = adc_value[i]; ClassifyIndex[1]++; }else if((adc_value[i]<=CpModuleBoundary.CpP9VLow)&&adc_value[i]>CpModuleBoundary.CpP9VHigh){ CPTempBuffer[2][ClassifyIndex[2]] = adc_value[i]; ClassifyIndex[2]++; }else if((adc_value[i]<=CpModuleBoundary.CpP6VLow)&&(adc_value[i]>CpModuleBoundary.CpP6VHigh)){ CPTempBuffer[3][ClassifyIndex[3]] = adc_value[i]; ClassifyIndex[3]++; }else if((adc_value[i]<=CpModuleBoundary.CpP3VLow)&&(adc_value[i]>CpModuleBoundary.CpP3VHigh)){ CPTempBuffer[4][ClassifyIndex[4]] = adc_value[i]; ClassifyIndex[4]++; #ifdef FUNC_CP_CLASSIFY_TO_OV_RANGE }else if((adc_value[i]<=CpModuleBoundary.Cp0VLow)&&(adc_value[i]>CpModuleBoundary.Cp0VHigh)){ CPTempBuffer[5][ClassifyIndex[5]] = adc_value[i]; ClassifyIndex[5]++; #endif }else if((adc_value[i]<=CpModuleBoundary.CpN12VLow)&&(adc_value[i]>CpModuleBoundary.CpN12VHigh)){ CPTempBuffer[6][ClassifyIndex[6]] = adc_value[i]; ClassifyIndex[6]++; }else{ //NUDTB CPTempBuffer[7][ClassifyIndex[7]] = adc_value[i]; ClassifyIndex[7]++; } } #ifdef TRACE_CP uint16_t sum = 0; { for (uint16_t i = 0; i < MAX_CLASSIFY_SECTION; i++) { sum += ClassifyIndex[i]; } } //XP("%4d %4d %4d %4d %4d %4d %4d %4d\r\n", XP("[>=12VH]%4d [12V]%4d [9V]%4d [6V]%4d [3V]%4d [0V]%4d [-12V]%4d [OTHERS]%4d (%d)\r\n", ClassifyIndex[0], ClassifyIndex[1], ClassifyIndex[2], ClassifyIndex[3], ClassifyIndex[4], ClassifyIndex[5], ClassifyIndex[6], ClassifyIndex[7], sum); static long EQ_Sum = 0; static long EQ_NG1 = 0; static long EQ_NG2 = 0; EQ_Sum++; #endif //check if 2 index count were the same IndexCounterEqual = false; #ifdef FUNC_CP_CLASSIFY_TO_OV_RANGE for(i=0;i<6;i++){ for(j=0;j<6;j++){ #else for(i=0;i<5;i++){ for(j=0;j<5;j++){ #endif #ifndef TRACE_CP if((ClassifyIndex[i]==ClassifyIndex[j])&&(ClassifyIndex[i]>0)&&(i!=j)){ #else //Make sure 6V => 9V, Relay must cut off in 100 ms ( < 60 ms is much better) //if((ClassifyIndex[i]==ClassifyIndex[j])&&(ClassifyIndex[i]>0)&&(i!=j)){ //60% NG //if((ClassifyIndex[i]==ClassifyIndex[j])&&(ClassifyIndex[i]>1)&&(i!=j)){ // 5% NG if((ClassifyIndex[i]==ClassifyIndex[j])&&(ClassifyIndex[i]>2)&&(i!=j)){ // 0% NG //if((ClassifyIndex[i]==ClassifyIndex[j])&&(ClassifyIndex[i]>7)&&(i!=j)){ // 0% NG #endif IndexCounterEqual = true; #ifdef TRACE_CP EQ_NG1++; #endif } } } if((ClassifyIndex[6]==ClassifyIndex[7])&&(ClassifyIndex[6]!=0)&&(ClassifyIndex[7]!=0)){ IndexCounterEqual = true; #ifdef TRACE_CP EQ_NG2++; #endif } //find the positive maxinum if(!IndexCounterEqual){ ClassifyIndexMax = ClassifyIndex[0]; MaxSection = 0; result->PositiveValue = 0; result->NegativeValue = 0; #ifdef FUNC_CP_CLASSIFY_TO_OV_RANGE for(i=0;i<6;){ #else for(i=0;i<5;){ #endif if(ClassifyIndex[i]>ClassifyIndexMax){ MaxSection = i; ClassifyIndexMax = ClassifyIndex[i]; } i++; } result->MaxPTB = MaxSection; result->MaxPTBCount = ClassifyIndex[MaxSection]; //check PTB_max is PUDTB if(MaxSection==0){ FlagStateUnknown = true; } //check NTB_max is NUDTB if(ClassifyIndex[7]>ClassifyIndex[6]) { #ifdef MODIFY_CP_NEG_JUDGMENT_PROCESS if (((float)(ClassifyIndex[7] - ClassifyIndex[6])) / (ClassifyIndex[7] + ClassifyIndex[6]) > 0.5f) { FlagStateUnknown = true; } #else FlagStateUnknown = true; #endif result->MaxNTB = 7; result->MaxNTBCount = ClassifyIndex[7]; }else{ result->MaxNTB = 6; result->MaxNTBCount = ClassifyIndex[6]; } // calculate PTB_max sum & average PositiveAdcSectionAvg = 0; PositiveAdcSectionSum = 0; MaxSection = result->MaxPTB; if(ClassifyIndex[MaxSection]>0){ for(j=0;j0){ PositiveAdcSectionAvg = (PositiveAdcSectionSum/ClassifyIndex[MaxSection]); //// if (PositiveAdcSectionAvg < 81) //// { //// XP("###PositiveAdcSectionAvg = %d", PositiveAdcSectionAvg); //// } } result->PositiveValue = PositiveAdcSectionAvg; } //calculate NTB sum & average NegativeAdcSectionAvg = 0; NegativeAdcSectionSum = 0; //if(CPTempBuffer[6]>0){ if(ClassifyIndex[6]>0){ //Hao@20210112: Revise previous line for(j=0;j0){ NegativeAdcSectionAvg = (NegativeAdcSectionSum/ClassifyIndex[6]); } result->NegativeValue = NegativeAdcSectionAvg; } if(!FlagStateUnknown) { //decide the new state switch(CpPreviousState){ case SYSTEM_STATE_A: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh+CpModuleHysteresis.HysteresisP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; case SYSTEM_STATE_B: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow-CpModuleHysteresis.HysteresisP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh+CpModuleHysteresis.HysteresisP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; case SYSTEM_STATE_C: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow-CpModuleHysteresis.HysteresisP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh+CpModuleHysteresis.HysteresisP3VHigh)){ CpNewState = SYSTEM_STATE_D; #ifdef FUNC_CP_VARIABLE_MAX_STATE_COUNTER_C_TO_D_SET_3_FOR_PASS_TERTEC_TEST MaxStateCounter = 3; #endif }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; case SYSTEM_STATE_D: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow-CpModuleHysteresis.HysteresisP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh+CpModuleHysteresis.Hysteresis0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; //------------------------------------------------------------------------------- [ New Added ] #ifdef MODIFY_UNKNOWN_TO_STATE_E case SYSTEM_STATE_E: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow-CpModuleHysteresis.HysteresisP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh)){ CpNewState = SYSTEM_STATE_E; #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; #endif //MODIFY_UNKNOWN_TO_STATE_E //------------------------------------------------------------------------------- #ifdef MODIFY_STATE_F_HYSTERESIS case SYSTEM_STATE_F: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; #else //MODIFY_STATE_F_HYSTERESIS case SYSTEM_STATE_F: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow-CpModuleHysteresis.HysteresisP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh+CpModuleHysteresis.HysteresisP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow-CpModuleHysteresis.HysteresisP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh+CpModuleHysteresis.HysteresisP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow-CpModuleHysteresis.HysteresisP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh+CpModuleHysteresis.HysteresisP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow-CpModuleHysteresis.HysteresisP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh+CpModuleHysteresis.HysteresisP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow-CpModuleHysteresis.Hysteresis0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh+CpModuleHysteresis.Hysteresis0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; #endif //MODIFY_STATE_F_HYSTERESIS #ifdef MODIFY_STATE_UNKNOWN_HYSTERESIS case SYSTEM_STATE_UNKNOWN: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow-CpModuleHysteresis.HysteresisP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh+CpModuleHysteresis.HysteresisP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow-CpModuleHysteresis.Hysteresis0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh+CpModuleHysteresis.Hysteresis0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; #else //MODIFY_STATE_UNKNOWN_HYSTERESIS case SYSTEM_STATE_UNKNOWN: if((PositiveAdcSectionAvg<=SpecBoundary.CpP12VLow-CpModuleHysteresis.HysteresisP12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP12VHigh+CpModuleHysteresis.HysteresisP12VHigh)){ CpNewState = SYSTEM_STATE_A; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP9VLow-CpModuleHysteresis.HysteresisP9VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP9VHigh+CpModuleHysteresis.HysteresisP9VHigh)){ CpNewState = SYSTEM_STATE_B; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP6VLow-CpModuleHysteresis.HysteresisP6VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP6VHigh+CpModuleHysteresis.HysteresisP6VHigh)){ CpNewState = SYSTEM_STATE_C; }else if((PositiveAdcSectionAvg<=SpecBoundary.CpP3VLow-CpModuleHysteresis.HysteresisP3VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpP3VHigh+CpModuleHysteresis.HysteresisP3VHigh)){ CpNewState = SYSTEM_STATE_D; }else if((PositiveAdcSectionAvg<=SpecBoundary.Cp0VLow-CpModuleHysteresis.Hysteresis0VLow)&&(PositiveAdcSectionAvg>SpecBoundary.Cp0VHigh+CpModuleHysteresis.Hysteresis0VHigh)){ #ifdef MODIFY_UNKNOWN_TO_STATE_E CpNewState = SYSTEM_STATE_E; #else CpNewState = SYSTEM_STATE_UNKNOWN; #endif #ifdef MODIFY_UNKNOWN_TO_STATE_F }else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; #else /*}else if((PositiveAdcSectionAvg<=SpecBoundary.CpN12VLow)&&(PositiveAdcSectionAvg>SpecBoundary.CpN12VHigh)){ CpNewState = SYSTEM_STATE_F; */ #endif }else{ CpNewState = SYSTEM_STATE_UNKNOWN; } break; #endif //MODIFY_STATE_UNKNOWN_HYSTERESIS #ifndef MODIFY_UNKNOWN_TO_STATE_E case SYSTEM_STATE_E: #endif default: break; } } else { #ifdef MODIFY_CP_STATE_F_SAMPLE_NUM_THRESHOLD if(ClassifyIndex[6] >= CP_STATE_F_SAMPLE_NUM_THRESHOLD) #else //if(ClassifyIndex[6]>=998) #endif { CpNewState = SYSTEM_STATE_F; } else { CpNewState = SYSTEM_STATE_UNKNOWN; } } #ifdef FUNC_GET_TRIP_TIME if (CpPreviousState == SYSTEM_STATE_C && CpNewState != SYSTEM_STATE_C) { Charger.m_TripBeg = HAL_GetTick(); Charger.m_TripEnd = HTK_U32_MAX; } #endif //new state filter if(CpNewState == CpPreviousState){ CpNewStateCounter++; #ifdef FUNC_CP_VARIABLE_MAX_STATE_COUNTER if(CpNewStateCounter >= MaxStateCounter) #else if(CpNewStateCounter >= MAX_STATE_COUNTER) #endif { result->State = CpNewState; CpNewStateCounter = 0; } }else{ CpNewStateCounter = 0; } CpPreviousState = CpNewState; #ifdef TRACE_CP TRACE_CP_Print_CPModuleResult(result, true, EQ_Sum, EQ_NG1, EQ_NG2); #endif } #ifdef TRACE_CP else { TRACE_CP_Print_CPModuleResult(result, false, EQ_Sum, EQ_NG1, EQ_NG2); } #endif } /* USER CODE END 0 */