From 7880acec98984ff769419969d9b7287c036438f6 Mon Sep 17 00:00:00 2001 From: rusefi Date: Sat, 2 Feb 2019 01:45:26 -0500 Subject: [PATCH] CJ125 unit-tests coverage #617 --- firmware/hw_layer/sensors/CJ125.cpp | 136 ++++++---------------- firmware/hw_layer/sensors/CJ125.h | 10 -- firmware/hw_layer/sensors/CJ125_logic.cpp | 55 ++++++++- firmware/hw_layer/sensors/CJ125_logic.h | 32 +++++ 4 files changed, 122 insertions(+), 111 deletions(-) diff --git a/firmware/hw_layer/sensors/CJ125.cpp b/firmware/hw_layer/sensors/CJ125.cpp index 9472597703..14be32253d 100644 --- a/firmware/hw_layer/sensors/CJ125.cpp +++ b/firmware/hw_layer/sensors/CJ125.cpp @@ -10,7 +10,6 @@ #include "CJ125.h" #include "pwm_generator.h" #include "rpm_calculator.h" -#include "pid.h" #if (EFI_CJ125 && HAL_USE_SPI) || defined(__DOXYGEN__) @@ -33,10 +32,6 @@ static Logging *logger; static unsigned char tx_buff[2]; static unsigned char rx_buff[1]; -static pid_s heaterPidConfig; -static Pid heaterPid(&heaterPidConfig); - -// todo: only define this variable in EIF_PROD static CJ125 globalInstance; static THD_WORKING_AREA(cjThreadStack, UTILITY_THREAD_STACK_SIZE); @@ -47,20 +42,9 @@ static SPIConfig cj125spicfg = { NULL, /* HW dependent part.*/ NULL, 0, SPI_CR1_MSTR | SPI_CR1_CPHA | SPI_CR1_BR_0 | SPI_CR1_BR_1 | SPI_CR1_BR_2 }; -// Current values -static volatile float vUa = 0.0f; -static volatile float vUr = 0.0f; -// Calibration values -static volatile float vUaCal = 0.0f, vUrCal = 0.0f; static volatile int lastSlowAdcCounter = 0; -static volatile cj125_mode_e mode = CJ125_MODE_NONE; -// Amplification coefficient, needed by cjGetAfr() -static volatile float amplCoeff = 0.0f; -// Calculated Lambda-value -static volatile float lambda = 1.0f; - // LSU conversion tables. See cj125_sensor_type_e // For LSU4.2, See http://www.bosch-motorsport.com/media/catalog_resources/Lambda_Sensor_LSU_42_Datasheet_51_en_2779111435pdf.pdf // See LSU4.9, See http://www.bosch-motorsport.com/media/catalog_resources/Lambda_Sensor_LSU_49_Datasheet_51_en_2779147659pdf.pdf @@ -165,7 +149,7 @@ static uint32_t get16bitFromVoltage(float v) { static void cjPrintData(void) { #ifdef CJ125_DEBUG - scheduleMsg(logger, "cj125: state=%d diag=0x%x (vUa=%.3f vUr=%.3f) (vUaCal=%.3f vUrCal=%.3f)", state, globalInstance.diag, vUa, vUr, vUaCal, vUrCal); + scheduleMsg(logger, "cj125: state=%d diag=0x%x (vUa=%.3f vUr=%.3f) (vUaCal=%.3f vUrCal=%.3f)", state, globalInstance.diag, vUa, vUr, globalInstance.vUaCal, globalInstance.vUrCal); #endif } @@ -191,28 +175,6 @@ static void cjPrintErrorCode(cj125_error_e errCode) { scheduleMsg(logger, "cj125 ERROR: %s.", errString); } -static void cjSetMode(cj125_mode_e m) { - if (mode == m) - return; - switch (m) { - case CJ125_MODE_NORMAL_8: - cjWriteRegister(INIT_REG1_WR, CJ125_INIT1_NORMAL_8); - amplCoeff = 1.0f / 8.0f; - break; - case CJ125_MODE_NORMAL_17: - cjWriteRegister(INIT_REG1_WR, CJ125_INIT1_NORMAL_17); - amplCoeff = 1.0f / 17.0f; - break; - case CJ125_MODE_CALIBRATION: - cjWriteRegister(INIT_REG1_WR, CJ125_INIT1_CALBRT); - amplCoeff = 0.0f; - break; - default: - ; - } - mode = m; -} - class RealSpi : public Cj125SpiStream { public: uint8_t ReadRegister(uint8_t reg) override { @@ -231,8 +193,8 @@ static void cjUpdateAnalogValues() { // todo: some solution for testing waitForSlowAdc(lastSlowAdcCounter); #endif - vUr = getUr(); - vUa = getUa(); + globalInstance.vUr = getUr(); + globalInstance.vUa = getUa(); #if EFI_PROD_CODE // todo: some solution for testing lastSlowAdcCounter = getSlowAdcCounter(); @@ -243,12 +205,12 @@ static void cjCalibrate(void) { globalInstance.cjIdentify(); scheduleMsg(logger, "cj125: Starting calibration..."); - cjSetMode(CJ125_MODE_CALIBRATION); + globalInstance.cjSetMode(CJ125_MODE_CALIBRATION); int init1 = cjReadRegister(INIT_REG1_RD); // check if our command has been accepted if (init1 != CJ125_INIT1_CALBRT) { scheduleMsg(logger, "cj125: Calibration error (init1=0x%02x)! Failed!", init1); - cjSetMode(CJ125_MODE_NORMAL_17); + globalInstance.cjSetMode(CJ125_MODE_NORMAL_17); return; } #if EFI_PROD_CODE @@ -256,8 +218,8 @@ static void cjCalibrate(void) { // wait for the start of the calibration chThdSleepMilliseconds(CJ125_CALIBRATION_DELAY); #endif - vUaCal = 0.0f; - vUrCal = 0.0f; + globalInstance.vUaCal = 0.0f; + globalInstance.vUrCal = 0.0f; // wait for some more ADC samples for (int i = 0; i < CJ125_CALIBRATE_NUM_SAMPLES; i++) { cjUpdateAnalogValues(); @@ -269,14 +231,14 @@ static void cjCalibrate(void) { } #endif /* EFI_TUNER_STUDIO */ - vUaCal += vUa; - vUrCal += vUr; + globalInstance.vUaCal += globalInstance.vUa; + globalInstance.vUrCal += globalInstance.vUr; } // find average - vUaCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES; - vUrCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES; + globalInstance.vUaCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES; + globalInstance.vUrCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES; // restore normal mode - cjSetMode(CJ125_MODE_NORMAL_17); + globalInstance.cjSetMode(CJ125_MODE_NORMAL_17); #if EFI_PROD_CODE // todo: testing solution chThdSleepMilliseconds(CJ125_CALIBRATION_DELAY); @@ -287,8 +249,8 @@ static void cjCalibrate(void) { cjPrintData(); // store new calibration data - uint32_t storedLambda = get16bitFromVoltage(vUaCal); - uint32_t storedHeater = get16bitFromVoltage(vUrCal); + uint32_t storedLambda = get16bitFromVoltage(globalInstance.vUaCal); + uint32_t storedHeater = get16bitFromVoltage(globalInstance.vUrCal); scheduleMsg(logger, "cj125: Done! Saving calibration data (%d %d).", storedLambda, storedHeater); #if EFI_PROD_CODE backupRamSave(BACKUP_CJ125_CALIBRATION_LAMBDA, storedLambda); @@ -319,10 +281,10 @@ static void cjStart(DECLARE_ENGINE_PARAMETER_SIGNATURE) { cjCalibrate(); } else { scheduleMsg(logger, "cj125: Loading stored calibration data (%d %d)", storedLambda, storedHeater); - vUaCal = getVoltageFrom16bit(storedLambda); - vUrCal = getVoltageFrom16bit(storedHeater); + globalInstance.vUaCal = getVoltageFrom16bit(storedLambda); + globalInstance.vUrCal = getVoltageFrom16bit(storedHeater); // Start normal measurement mode - cjSetMode(CJ125_MODE_NORMAL_17); + globalInstance.cjSetMode(CJ125_MODE_NORMAL_17); } cjPrintData(); @@ -343,23 +305,6 @@ void CJ125::setError(cj125_error_e errCode DECLARE_ENGINE_PARAMETER_SUFFIX) { cjWriteRegister(INIT_REG2_WR, CJ125_INIT2_RESET); } -static void cjInitPid(DECLARE_ENGINE_PARAMETER_SIGNATURE) { - if(engineConfiguration->cj125isLsu49) { - heaterPidConfig.pFactor = CJ125_PID_LSU49_P; - heaterPidConfig.iFactor = CJ125_PID_LSU49_I; - } else { - heaterPidConfig.pFactor = CJ125_PID_LSU42_P; - heaterPidConfig.iFactor = CJ125_PID_LSU42_I; - } - heaterPidConfig.dFactor = 0.0f; - heaterPidConfig.minValue = 0; - heaterPidConfig.maxValue = 1; - heaterPidConfig.offset = 0; - // todo: period? - heaterPidConfig.period = 1.0f; - heaterPid.reset(); -} - // engineConfiguration->spi2SckMode = PAL_STM32_OTYPE_OPENDRAIN; // 4 // engineConfiguration->spi2MosiMode = PAL_STM32_OTYPE_OPENDRAIN; // 4 // engineConfiguration->spi2MisoMode = PAL_STM32_PUDR_PULLUP; // 32 @@ -414,23 +359,23 @@ static bool cj125periodic(CJ125 *instance DECLARE_ENGINE_PARAMETER_SUFFIX) { cjCalibrate(); // Start normal operation instance->state = CJ125_INIT; - cjSetMode(CJ125_MODE_NORMAL_17); + globalInstance.cjSetMode(CJ125_MODE_NORMAL_17); } globalInstance.diag = cjReadRegister(DIAG_REG_RD); // check heater state - if (vUr > CJ125_UR_PREHEAT_THR || instance->heaterDuty < CJ125_PREHEAT_MIN_DUTY) { + if (globalInstance.vUr > CJ125_UR_PREHEAT_THR || instance->heaterDuty < CJ125_PREHEAT_MIN_DUTY) { // Check if RPM>0 and it's time to start pre-heating if (instance->state == CJ125_INIT && !isStopped) { // start preheating instance->state = CJ125_PREHEAT; instance->startHeatingNt = instance->prevNt = getTimeNowNt(); - cjSetMode(CJ125_MODE_NORMAL_17); + globalInstance.cjSetMode(CJ125_MODE_NORMAL_17); } - } else if (vUr > CJ125_UR_GOOD_THR) { + } else if (instance->vUr > CJ125_UR_GOOD_THR) { instance->state = CJ125_HEAT_UP; - } else if (vUr < CJ125_UR_OVERHEAT_THR) { + } else if (instance->vUr < CJ125_UR_OVERHEAT_THR) { instance->state = CJ125_OVERHEAT; } else { // This indicates that the heater temperature is optimal for UA measurement @@ -444,7 +389,7 @@ static bool cj125periodic(CJ125 *instance DECLARE_ENGINE_PARAMETER_SUFFIX) { #if 0 // Change amplification if AFR gets lean/rich for better accuracy - cjSetMode(lambda > 1.0f ? CJ125_MODE_NORMAL_17 : CJ125_MODE_NORMAL_8); + globalInstance.cjSetMode(globalInstance.lambda > 1.0f ? CJ125_MODE_NORMAL_17 : CJ125_MODE_NORMAL_8); #endif switch (instance->state) { @@ -471,8 +416,8 @@ static bool cj125periodic(CJ125 *instance DECLARE_ENGINE_PARAMETER_SUFFIX) { * error value as the difference of (target - input). and if we swap them we'll just get a sign inversion. If target=vUrCal, and input=vUr, then error=vUrCal-vUr, i.e. if vUrSetHeater(duty PASS_ENGINE_PARAMETER_SUFFIX); cjPrintData(); instance->prevNt = nowNt; @@ -556,37 +501,28 @@ float cjGetAfr(DECLARE_ENGINE_PARAMETER_SIGNATURE) { } // See CJ125 datasheet, page 6 - float pumpCurrent = (vUa - vUaCal) * amplCoeff * (CJ125_PUMP_CURRENT_FACTOR / CJ125_PUMP_SHUNT_RESISTOR); - lambda = interpolate2d("cj125Lsu", pumpCurrent, (float *)cjLSUBins[sensorType], (float *)cjLSULambda[sensorType], cjLSUTableSize[sensorType]); + float pumpCurrent = (globalInstance.vUa - globalInstance.vUaCal) * globalInstance.amplCoeff * (CJ125_PUMP_CURRENT_FACTOR / CJ125_PUMP_SHUNT_RESISTOR); + globalInstance.lambda = interpolate2d("cj125Lsu", pumpCurrent, (float *)cjLSUBins[sensorType], (float *)cjLSULambda[sensorType], cjLSUTableSize[sensorType]); // todo: make configurable stoich ratio - return lambda * CJ125_STOICH_RATIO; + return globalInstance.lambda * CJ125_STOICH_RATIO; } bool cjHasAfrSensor(DECLARE_ENGINE_PARAMETER_SIGNATURE) { if (!CONFIGB(isCJ125Enabled)) return false; - // check if controller is functioning - if (!globalInstance.isWorkingState()) - return false; - // check if amplification is turned on - if (amplCoeff == 0.0f) - return false; - // check if UA calibration value is valid - if (vUaCal < CJ125_UACAL_MIN || vUaCal > CJ125_UACAL_MAX) - return false; - return true; + return globalInstance.isValidState(); } #if EFI_TUNER_STUDIO || defined(__DOXYGEN__) // used by DBG_CJ125 void cjPostState(TunerStudioOutputChannels *tsOutputChannels) { tsOutputChannels->debugFloatField1 = globalInstance.heaterDuty; - tsOutputChannels->debugFloatField2 = heaterPid.getIntegration(); - tsOutputChannels->debugFloatField3 = heaterPid.getPrevError(); - tsOutputChannels->debugFloatField4 = vUa; - tsOutputChannels->debugFloatField5 = vUr; - tsOutputChannels->debugFloatField6 = vUaCal; - tsOutputChannels->debugFloatField7 = vUrCal; + tsOutputChannels->debugFloatField2 = globalInstance.heaterPid.getIntegration(); + tsOutputChannels->debugFloatField3 = globalInstance.heaterPid.getPrevError(); + tsOutputChannels->debugFloatField4 = globalInstance.vUa; + tsOutputChannels->debugFloatField5 = globalInstance.vUr; + tsOutputChannels->debugFloatField6 = globalInstance.vUaCal; + tsOutputChannels->debugFloatField7 = globalInstance.vUrCal; tsOutputChannels->debugIntField1 = globalInstance.state; tsOutputChannels->debugIntField2 = globalInstance.diag; } @@ -610,7 +546,7 @@ void initCJ125(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) { return; } - cjInitPid(PASS_ENGINE_PARAMETER_SIGNATURE); + globalInstance.cjInitPid(PASS_ENGINE_PARAMETER_SIGNATURE); cjStartSpi(PASS_ENGINE_PARAMETER_SIGNATURE); scheduleMsg(logger, "cj125: Starting heater control"); globalInstance.StartHeaterControl(applyPinState PASS_ENGINE_PARAMETER_SUFFIX); diff --git a/firmware/hw_layer/sensors/CJ125.h b/firmware/hw_layer/sensors/CJ125.h index 7c7ec03c42..7f0fc7ae93 100644 --- a/firmware/hw_layer/sensors/CJ125.h +++ b/firmware/hw_layer/sensors/CJ125.h @@ -84,9 +84,6 @@ #define CJ125_UR_OVERHEAT_THR 0.5f // Ur < 0.5 Volts is overheat #define CJ125_UR_GOOD_THR 1.4f -#define CJ125_UACAL_MIN 1.0f // Calibration UA values range -#define CJ125_UACAL_MAX 2.0f - #define CJ125_PREHEAT_MIN_DUTY 0.9f //#define CJ125_PREHEAT_TIMEOUT 90 // 90 secs @@ -98,13 +95,6 @@ #define CJ125_STOICH_RATIO 14.7f #define CJ125_PUMP_CURRENT_FACTOR 1000.0f -// Some experimental magic values for heater PID regulator -#define CJ125_PID_LSU42_P (80.0f / 16.0f * 5.0f / 1024.0f) -#define CJ125_PID_LSU42_I (25.0f / 16.0f * 5.0f / 1024.0f) - -#define CJ125_PID_LSU49_P (8.0f) -#define CJ125_PID_LSU49_I (0.003f) - // Returned if there's no valid measurement #define CJ125_AFR_NAN 0.0f diff --git a/firmware/hw_layer/sensors/CJ125_logic.cpp b/firmware/hw_layer/sensors/CJ125_logic.cpp index 8aaa2ada50..b0f12d66e7 100644 --- a/firmware/hw_layer/sensors/CJ125_logic.cpp +++ b/firmware/hw_layer/sensors/CJ125_logic.cpp @@ -10,7 +10,8 @@ EXTERN_ENGINE; -CJ125::CJ125() : wboHeaterControl("wbo") { +CJ125::CJ125() : wboHeaterControl("wbo"), + heaterPid(&heaterPidConfig){ } void CJ125::SetHeater(float value DECLARE_ENGINE_PARAMETER_SUFFIX) { @@ -71,3 +72,55 @@ void CJ125::cjIdentify(void) { } #endif } + +void CJ125::cjSetMode(cj125_mode_e m) { + if (mode == m) + return; + switch (m) { + case CJ125_MODE_NORMAL_8: + spi->WriteRegister(INIT_REG1_WR, CJ125_INIT1_NORMAL_8); + amplCoeff = 1.0f / 8.0f; + break; + case CJ125_MODE_NORMAL_17: + spi->WriteRegister(INIT_REG1_WR, CJ125_INIT1_NORMAL_17); + amplCoeff = 1.0f / 17.0f; + break; + case CJ125_MODE_CALIBRATION: + spi->WriteRegister(INIT_REG1_WR, CJ125_INIT1_CALBRT); + amplCoeff = 0.0f; + break; + default: + ; + } + mode = m; +} + +bool CJ125::isValidState() { + // check if controller is functioning + if (!isWorkingState()) + return false; + // check if amplification is turned on + if (amplCoeff == 0.0f) + return false; + // check if UA calibration value is valid + if (vUaCal < CJ125_UACAL_MIN || vUaCal > CJ125_UACAL_MAX) + return false; + return true; +} + +void CJ125::cjInitPid(DECLARE_ENGINE_PARAMETER_SIGNATURE) { + if(engineConfiguration->cj125isLsu49) { + heaterPidConfig.pFactor = CJ125_PID_LSU49_P; + heaterPidConfig.iFactor = CJ125_PID_LSU49_I; + } else { + heaterPidConfig.pFactor = CJ125_PID_LSU42_P; + heaterPidConfig.iFactor = CJ125_PID_LSU42_I; + } + heaterPidConfig.dFactor = 0.0f; + heaterPidConfig.minValue = 0; + heaterPidConfig.maxValue = 1; + heaterPidConfig.offset = 0; + // todo: period? + heaterPidConfig.period = 1.0f; + heaterPid.reset(); +} diff --git a/firmware/hw_layer/sensors/CJ125_logic.h b/firmware/hw_layer/sensors/CJ125_logic.h index 5aa4d92376..7da3056eed 100644 --- a/firmware/hw_layer/sensors/CJ125_logic.h +++ b/firmware/hw_layer/sensors/CJ125_logic.h @@ -10,6 +10,7 @@ #include "engine_configuration.h" #include "pwm_generator_logic.h" +#include "pid.h" typedef enum { CJ125_LSU_42 = 0, @@ -65,6 +66,24 @@ public: efitick_t prevNt; float heaterDuty = 0.0f; + pid_s heaterPidConfig; + Pid heaterPid; + + volatile cj125_mode_e mode = CJ125_MODE_NONE; + + // Amplification coefficient, needed by cjGetAfr() + volatile float amplCoeff = 0.0f; + // Calculated Lambda-value + volatile float lambda = 1.0f; + + // Current values + volatile float vUa = 0.0f; + volatile float vUr = 0.0f; + + // Calibration values + volatile float vUaCal = 0.0f; + volatile float vUrCal = 0.0f; + OutputPin wboHeaterPin; OutputPin cj125Cs; @@ -79,6 +98,9 @@ public: void SetIdleHeater(DECLARE_ENGINE_PARAMETER_SIGNATURE); void StartHeaterControl(pwm_gen_callback *stateChangeCallback DECLARE_ENGINE_PARAMETER_SUFFIX); void cjIdentify(void); + void cjSetMode(cj125_mode_e m); + bool isValidState(); + void cjInitPid(DECLARE_ENGINE_PARAMETER_SIGNATURE); }; // Heater params for Idle(cold), Preheating and Control stages @@ -116,4 +138,14 @@ public: #define CJ125_DIAG_NORM 0xFF // no errors +#define CJ125_UACAL_MIN 1.0f // Calibration UA values range +#define CJ125_UACAL_MAX 2.0f + +// Some experimental magic values for heater PID regulator +#define CJ125_PID_LSU42_P (80.0f / 16.0f * 5.0f / 1024.0f) +#define CJ125_PID_LSU42_I (25.0f / 16.0f * 5.0f / 1024.0f) + +#define CJ125_PID_LSU49_P (8.0f) +#define CJ125_PID_LSU49_I (0.003f) + #endif /* HW_LAYER_SENSORS_CJ125_LOGIC_H_ */