CJ125 unit-tests coverage #617

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();
+	globalInstance.vUr = getUr();
-    vUa = getUa();
+	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;
+	globalInstance.vUaCal = 0.0f;
-	vUrCal = 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;
+		globalInstance.vUaCal += globalInstance.vUa;
-		vUrCal += vUr;
+		globalInstance.vUrCal += globalInstance.vUr;
 	}
 	// find average
-	vUaCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES;
+	globalInstance.vUaCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES;
-	vUrCal /= (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 storedLambda = get16bitFromVoltage(globalInstance.vUaCal);
-	uint32_t storedHeater = get16bitFromVoltage(vUrCal);
+	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);
+		globalInstance.vUaCal = getVoltageFrom16bit(storedLambda);
-		vUrCal = getVoltageFrom16bit(storedHeater);
+		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 vUr<vUrCal then the error will cause the heater to increase it's duty cycle. But it's not exactly what we want! Lesser vUr means HOTTER cell. That's why we even have this safety check for overheating: (vUr < CJ125_UR_OVERHEAT_THR)...
 				 * So the simple trick is to inverse the error by swapping the target and input values.
 				 */
-				float duty = heaterPid.getValue(vUr, vUrCal);
+				float duty = globalInstance.heaterPid.getValue(globalInstance.vUr, globalInstance.vUrCal);
-				heaterPid.showPidStatus(logger, "cj");
+				globalInstance.heaterPid.showPidStatus(logger, "cj");
 				instance->SetHeater(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);
+	float pumpCurrent = (globalInstance.vUa - globalInstance.vUaCal) * globalInstance.amplCoeff * (CJ125_PUMP_CURRENT_FACTOR / CJ125_PUMP_SHUNT_RESISTOR);
-	lambda = interpolate2d("cj125Lsu", pumpCurrent, (float *)cjLSUBins[sensorType], (float *)cjLSULambda[sensorType], cjLSUTableSize[sensorType]);
+	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
+	return globalInstance.isValidState();
-	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;
 }
 
 #if EFI_TUNER_STUDIO || defined(__DOXYGEN__)
 // used by DBG_CJ125
 void cjPostState(TunerStudioOutputChannels *tsOutputChannels) {
 	tsOutputChannels->debugFloatField1 = globalInstance.heaterDuty;
-	tsOutputChannels->debugFloatField2 = heaterPid.getIntegration();
+	tsOutputChannels->debugFloatField2 = globalInstance.heaterPid.getIntegration();
-	tsOutputChannels->debugFloatField3 = heaterPid.getPrevError();
+	tsOutputChannels->debugFloatField3 = globalInstance.heaterPid.getPrevError();
-	tsOutputChannels->debugFloatField4 = vUa;
+	tsOutputChannels->debugFloatField4 = globalInstance.vUa;
-	tsOutputChannels->debugFloatField5 = vUr;
+	tsOutputChannels->debugFloatField5 = globalInstance.vUr;
-	tsOutputChannels->debugFloatField6 = vUaCal;
+	tsOutputChannels->debugFloatField6 = globalInstance.vUaCal;
-	tsOutputChannels->debugFloatField7 = vUrCal;
+	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);

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
 

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();
+}

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_ */