From f82c56acf4f71c7a5a74e78ad2c7259dff4297c9 Mon Sep 17 00:00:00 2001 From: andreika-git Date: Mon, 29 Jan 2018 21:26:42 +0200 Subject: [PATCH] Support for CJ125 Wideband controller (alpha version) (#560) --- firmware/hw_layer/sensors/CJ125.cpp | 593 ++++++++++++++++++++++++---- firmware/hw_layer/sensors/CJ125.h | 107 ++++- 2 files changed, 614 insertions(+), 86 deletions(-) diff --git a/firmware/hw_layer/sensors/CJ125.cpp b/firmware/hw_layer/sensors/CJ125.cpp index 92b4e62e39..06833c39c0 100644 --- a/firmware/hw_layer/sensors/CJ125.cpp +++ b/firmware/hw_layer/sensors/CJ125.cpp @@ -1,8 +1,6 @@ /* * @file CJ125.cpp * - * Based on Jeff "Turbo SOB" work - * * @date: Jun 24, 2016 * @author Andrey Belomutskiy, (c) 2012-2018 * @@ -11,96 +9,352 @@ #include "engine.h" #include "CJ125.h" #include "pwm_generator.h" +#include "rpm_calculator.h" #include "pid.h" #if EFI_CJ125 || defined(__DOXYGEN__) #include "pin_repository.h" #include "hardware.h" +#include "adc_inputs.h" +#include "backup_ram.h" + + +#define CJ125_DEBUG +//#define CJ125_DEBUG_SPI EXTERN_ENGINE; -static SimplePwm wboHeaderControl; -static OutputPin wboHeaderPin; +#if ! EFI_UNIT_TEST || defined(__DOXYGEN__) +extern TunerStudioOutputChannels tsOutputChannels; +#endif + +static SimplePwm wboHeaterControl; +static OutputPin wboHeaterPin; static OutputPin cj125Cs; static Logging *logger; -static unsigned char tx_buff[1]; +static unsigned char tx_buff[2]; static unsigned char rx_buff[1]; +static pid_s heaterPidConfig; +static Pid heaterPid(&heaterPidConfig); +static float heaterDuty = 0.0f; + static THD_WORKING_AREA(cjThreadStack, UTILITY_THREAD_STACK_SIZE); static SPIDriver *driver; 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 }; + /* HW dependent part.*/ + NULL, 0, SPI_CR1_MSTR | SPI_CR1_CPHA | SPI_CR1_BR_0 | SPI_CR1_BR_1 | SPI_CR1_BR_2 }; -/********************************************************************************** - INIT_REG1 - CJ125 Initialization Register 1 - 00x000x0 - ||||||||---- VL - Pump Current Sense Amplifier - 0 = x8, 1 = x17 - If VL is set to "0" the amplification of Ip is 8. - If VL is "1" the amplification is 17. The higher - amplification is needed for a better resolution - at Lambda > 1, amplification 8 for Lambda < 1. - Note: It seems to make no difference, always set to 1. - |||||||----- Unused - ||||||------ LA - Pump Current Sense Amplifier - 0 = measurement, 1 = calibration - If LA is "0" the measurement mode for Ip is active. - The current pump current is displayed on Ua. If LA is - set to "1" the calibration mode is active. The shown - voltage on Ua must been subtracted from the later measurement. - |||||------- Undocumented Bit - Note: CJ125 only seems to work when set to 1. - ||||-------- RA - Measurement Current for Ri - 0 = measurement, 1 = calibration - If RA is "0" the measurement mode for Ri is active and - the current resistance of the probe is "displayed" on Ur. - If RA is "1" the calibration mode is active. You get the - optimal voltage for the inner resistance of the probe on Ur. - |||--------- Unused - ||---------- PA - Pump Current Control - Set to 0 to be active - |----------- ENABLE/HOLD - Must be set to 1 to enable -***********************************************************************************/ +// Used by CJ125 driver state machine +static volatile cj125_state_e state = CJ125_IDLE; +// Chip diagnostics register contents +static volatile int diag = 0; +// Last Error code +static volatile cj125_error_e errorCode = CJ125_ERROR_NONE; -static cj125_state_e state = CJ125_IDLE; +// Current values +static volatile float vUa = 0.0f, vUr = 0.0f; +// Calibration values +static volatile float vUaCal = 0.0f, vUrCal = 0.0f; -static msg_t cjThread(void) -{ - chRegSetThreadName("cj125"); +static volatile int lastSlowAdcCounter = 0; -// while(1) { +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; -// } - return -1; -} +// 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 +static const int CJ125_LSU_CURVE_SIZE = 16; +// This is a number of bins for each sensor type (should be < CJ125_LSU_CURVE_SIZE) +static const float cjLSUTableSize[2] = { + 9, 15, +}; +// Pump current, mA +static const float cjLSUBins[2][CJ125_LSU_CURVE_SIZE] = { { + // LSU 4.2 + -1.85f, -1.08f, -0.76f, -0.47f, 0.0f, 0.34f, 0.68f, 0.95f, 1.4f }, { + // LSU 4.9 + -2.0f, -1.602f, -1.243f, -0.927f, -0.8f, -0.652f, -0.405f, -0.183f, -0.106f, -0.04f, 0, 0.015f, 0.097f, 0.193f, 0.250f }, +}; +// Lambda value +static const float cjLSULambda[2][CJ125_LSU_CURVE_SIZE] = { { + // LSU 4.2 + 0.7f, 0.8f, 0.85f, 0.9f, 1.009f, 1.18f, 1.43f, 1.7f, 2.42f }, { + // LSU 4.9 + 0.65f, 0.7f, 0.75f, 0.8f, 0.822f, 0.85f, 0.9f, 0.95f, 0.97f, 0.99f, 1.003f, 1.01f, 1.05f, 1.1f, 1.132f }, +}; -static void cj125test(void) { - - // read identity command +static int cjReadRegister(unsigned char regAddr) { spiSelect(driver); - tx_buff[0] = IDENT_REG_RD; + tx_buff[0] = regAddr; spiSend(driver, 1, tx_buff); - + // safety? + chThdSleepMilliseconds(10); + + rx_buff[0] = 0; spiReceive(driver, 1, rx_buff); - int value = rx_buff[0] & 0xF8; - spiUnselect(driver); - scheduleMsg(logger, "cj125 got %x", value); - - - +#ifdef CJ125_DEBUG_SPI + scheduleMsg(logger, "cjReadRegister: addr=%d answer=%d", regAddr, rx_buff[0]); +#endif + return rx_buff[0]; } -void initCJ125(Logging *sharedLogger) { - logger = sharedLogger; - // still a lot to be done here :) +static void cjWriteRegister(unsigned char regAddr, unsigned char regValue) { +#ifdef CJ125_DEBUG_SPI + scheduleMsg(logger, "cjWriteRegister: addr=%d value=%d", regAddr, regValue); +#endif + spiSelect(driver); + tx_buff[0] = regAddr; + tx_buff[1] = regValue; + spiSend(driver, 2, tx_buff); + spiUnselect(driver); +} - if (!boardConfiguration->isCJ125Enabled) +static float getUr() { + if (CONFIG(cj125ur) != EFI_ADC_NONE) { +#ifdef EFI_CJ125_DIRECTLY_CONNECTED_UR + // in case of directly connected Ur signal from CJ125 to the ADC pin of MCU + return getVoltage("cj125ur", CONFIG(cj125ur)); +#else + // if a standard voltage division scheme with OpAmp is used + return getVoltageDivided("cj125ur", CONFIG(cj125ur)); +#endif + } + return 0.0f; +} + +static float getUa() { + if (CONFIG(cj125ua) != EFI_ADC_NONE) + return getVoltageDivided("cj125ua", CONFIG(cj125ua)); + return 0.0f; +} + +static float getVoltageFrom16bit(uint32_t stored) { + return ((float)stored) / CJ125_VOLTAGE_TO_16BIT_FACTOR; +} + +static uint32_t get16bitFromVoltage(float v) { + return (uint32_t)(v * CJ125_VOLTAGE_TO_16BIT_FACTOR); +} + +static void cjPrintData(void) { +#ifdef CJ125_DEBUG + scheduleMsg(logger, "cj125: state=%d diag=0x%x (vUa=%.3f vUr=%.3f) (vUaCal=%.3f vUrCal=%.3f)", state, diag, vUa, vUr, vUaCal, vUrCal); +#endif +} + +static void cjPrintErrorCode(cj125_error_e errCode) { + const char *errString = nullptr; + switch (errCode) { + case CJ125_ERROR_HEATER_MALFUNCTION: + errString = "Heater malfunction (Too long preheat)"; + break; + case CJ125_ERROR_OVERHEAT: + errString = "Sensor overheating"; + break; + case CJ125_ERROR_NONE: + errString = "N/A"; + break; + } + 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; +} +static void cjIdentify(void) { + // read Ident register + int ident = cjReadRegister(IDENT_REG_RD) & CJ125_IDENT_MASK; + + // set initial registers + cjWriteRegister(INIT_REG1_WR, CJ125_INIT1_NORMAL_17); + cjWriteRegister(INIT_REG2_WR, CJ125_INIT2_DIAG); + // check if regs are ok + int init1 = cjReadRegister(INIT_REG1_RD); + int init2 = cjReadRegister(INIT_REG2_RD); + + diag = cjReadRegister(DIAG_REG_RD); + scheduleMsg(logger, "cj125: Check ident=0x%x diag=0x%x init1=0x%x init2=0x%x", ident, diag, init1, init2); + if (ident != CJ125_IDENT) { + scheduleMsg(logger, "cj125: Error! Wrong ident! Cannot communicate with CJ125!"); + } + if (init1 != CJ125_INIT1_NORMAL_17 || init2 != CJ125_INIT2_DIAG) { + scheduleMsg(logger, "cj125: Error! Cannot set init registers! Cannot communicate with CJ125!"); + } +#if 0 + if (diag != CJ125_DIAG_NORM) { + scheduleMsg(logger, "cj125: Diag error!"); + } +#endif +} + +static void cjUpdateAnalogValues(void) { + waitForSlowAdc(lastSlowAdcCounter); + vUr = getUr(); + vUa = getUa(); + lastSlowAdcCounter = getSlowAdcCounter(); +} + +static void cjCalibrate(void) { + cjIdentify(); + + scheduleMsg(logger, "cj125: Starting calibration..."); + 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); + return; + } + // wait for the start of the calibration + chThdSleepMilliseconds(CJ125_CALIBRATION_DELAY); + vUaCal = 0.0f; + vUrCal = 0.0f; + // wait for some more ADC samples + for (int i = 0; i < CJ125_CALIBRATE_NUM_SAMPLES; i++) { + cjUpdateAnalogValues(); + cjPrintData(); + cjPostState(&tsOutputChannels); + vUaCal += vUa; + vUrCal += vUr; + } + // find average + vUaCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES; + vUrCal /= (float)CJ125_CALIBRATE_NUM_SAMPLES; + // restore normal mode + cjSetMode(CJ125_MODE_NORMAL_17); + chThdSleepMilliseconds(CJ125_CALIBRATION_DELAY); + // check if everything is ok + diag = cjReadRegister(DIAG_REG_RD); + cjUpdateAnalogValues(); + cjPrintData(); + + // store new calibration data + uint32_t storedLambda = get16bitFromVoltage(vUaCal); + uint32_t storedHeater = get16bitFromVoltage(vUrCal); + scheduleMsg(logger, "cj125: Done! Saving calibration data (%d %d).", storedLambda, storedHeater); + backupRamSave(BACKUP_CJ125_CALIBRATION_LAMBDA, storedLambda); + backupRamSave(BACKUP_CJ125_CALIBRATION_HEATER, storedHeater); + + state = CJ125_IDLE; +} + +static void cjStart(void) { + if (!boardConfiguration->isCJ125Enabled) { + scheduleMsg(logger, "cj125 is disabled."); + return; + } + + cjIdentify(); + + // Load calibration values + uint32_t storedLambda = backupRamLoad(BACKUP_CJ125_CALIBRATION_LAMBDA); + uint32_t storedHeater = backupRamLoad(BACKUP_CJ125_CALIBRATION_HEATER); + // if no calibration, try to calibrate now and store new values + if (storedLambda == 0 || storedHeater == 0) { + cjCalibrate(); + } else { + scheduleMsg(logger, "cj125: Loading stored calibration data (%d %d)", storedLambda, storedHeater); + vUaCal = getVoltageFrom16bit(storedLambda); + vUrCal = getVoltageFrom16bit(storedHeater); + // Start normal measurement mode + cjSetMode(CJ125_MODE_NORMAL_17); + } + cjPrintData(); + + lastSlowAdcCounter = getSlowAdcCounter(); +} + +static void cjSetHeater(float value) { + // limit duty cycle for sensor safety + float maxDuty = (engine->sensors.vBatt > CJ125_HEATER_LIMITING_VOLTAGE) ? CJ125_HEATER_LIMITING_RATE : 1.0f; + heaterDuty = (value < CJ125_HEATER_MIN_DUTY) ? 0.0f : minF(maxF(value, 0.0f), maxDuty); +#ifdef CJ125_DEBUG + scheduleMsg(logger, "cjSetHeater: %.2f", heaterDuty); +#endif + // a little trick to disable PWM if needed. + // todo: this should be moved to wboHeaterControl.setPwmDutyCycle() + wboHeaterControl.setFrequency(heaterDuty == 0.0f ? NAN : CJ125_HEATER_PWM_FREQ); + wboHeaterControl.setSimplePwmDutyCycle(heaterDuty); + // This fixes pwm sticking to the last pin state + if (heaterDuty == 0.0f) + wboHeaterPin.setValue(false); +} + +static void cjSetIdleHeater(void) { + // small preheat for faster start & moisture anti-shock therapy for the sensor + cjSetHeater(CJ125_HEATER_IDLE_RATE); +} + +static void cjStartHeaterControl(void) { + if (boardConfiguration->wboHeaterPin != GPIO_UNASSIGNED) { + scheduleMsg(logger, "cj125: Starting heater control"); + // todo: use custom pin state method, turn pin off while not running + startSimplePwmExt(&wboHeaterControl, "wboHeaterPin", boardConfiguration->wboHeaterPin, + &wboHeaterPin, CJ125_HEATER_PWM_FREQ, 0.0f, applyPinState); + cjSetIdleHeater(); + } +} + +static void cjSetError(cj125_error_e errCode) { + errorCode = errCode; + state = CJ125_ERROR; + cjPrintErrorCode(errorCode); + // This is for safety: + scheduleMsg(logger, "cj125: Controller Shutdown!"); + cjSetHeater(0); + // Software-reset of CJ125 + cjWriteRegister(INIT_REG2_WR, CJ125_INIT2_RESET); +} + +static bool cjIsWorkingState(void) { + return state != CJ125_ERROR && state != CJ125_INIT && state != CJ125_IDLE; +} + +static void cjInitPid(void) { + // todo: these values are valid only for LSU 4.2 + 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(); +} + +static void cjStartSpi(void) { cj125spicfg.ssport = getHwPort("cj125", boardConfiguration->cj125CsPin); cj125spicfg.sspad = getHwPin("cj125", boardConfiguration->cj125CsPin); @@ -109,33 +363,222 @@ void initCJ125(Logging *sharedLogger) { cj125Cs.initPin("cj125 CS", boardConfiguration->cj125CsPin, &engineConfiguration->cj125CsPinMode); - if (boardConfiguration->wboHeaterPin != GPIO_UNASSIGNED) { - // todo: use custom pin state method, turn pin off while not running - startSimplePwmExt(&wboHeaderControl, "heater control", boardConfiguration->wboHeaterPin, - &wboHeaderPin, - 300, 0.1, applyPinState); - - - } - - scheduleMsg(logger, "Starting cj125 spi driver"); + scheduleMsg(logger, "cj125: Starting SPI driver"); spiStart(driver, &cj125spicfg); - - addConsoleAction("cj125", cj125test); - - chThdCreateStatic(cjThreadStack, sizeof(cjThreadStack), LOWPRIO, (tfunc_t) cjThread, NULL); - } -void cjPostState(TunerStudioOutputChannels *tsOutputChannels) { +static msg_t cjThread(void) +{ + chRegSetThreadName("cj125"); + + chThdSleepMilliseconds(500); + + efitick_t startHeatingNt = 0; + efitick_t prevNt = getTimeNowNt(); + while(1) { + efitick_t nowNt = getTimeNowNt(); + bool isStopped = engine->rpmCalculator.isStopped(PASS_ENGINE_PARAMETER_SIGNATURE); + + cjUpdateAnalogValues(); + + // If the controller is disabled + if (state == CJ125_IDLE || state == CJ125_ERROR) { + chThdSleepMilliseconds(CJ125_IDLE_TICK_DELAY); + continue; + } + + if (state == CJ125_CALIBRATION) { + cjCalibrate(); + // Start normal operation + state = CJ125_INIT; + cjSetMode(CJ125_MODE_NORMAL_17); + } + + diag = cjReadRegister(DIAG_REG_RD); + + // check heater state + if (vUr > CJ125_UR_PREHEAT_THR || heaterDuty < CJ125_PREHEAT_MIN_DUTY) { + // Check if RPM>0 and it's time to start pre-heating + if (state == CJ125_INIT && !isStopped) { + // start preheating + state = CJ125_PREHEAT; + startHeatingNt = prevNt = getTimeNowNt(); + cjSetMode(CJ125_MODE_NORMAL_17); + } + } else if (vUr > CJ125_UR_GOOD_THR) { + state = CJ125_HEAT_UP; + } else if (vUr < CJ125_UR_OVERHEAT_THR) { + state = CJ125_OVERHEAT; + } else { + // This indicates that the heater temperature is optimal for UA measurement + state = CJ125_READY; + } + + if (isStopped && cjIsWorkingState()) { + state = CJ125_INIT; + cjSetIdleHeater(); + } + +#if 0 + // Change amplification if AFR gets lean/rich for better accuracy + cjSetMode(lambda > 1.0f ? CJ125_MODE_NORMAL_17 : CJ125_MODE_NORMAL_8); +#endif + + // Update console output variables + cjPostState(&tsOutputChannels); + + switch (state) { + case CJ125_PREHEAT: + // use constant-speed startup heat-up + if (nowNt - prevNt >= CJ125_HEATER_PREHEAT_PERIOD) { + float periodSecs = (float)(nowNt - prevNt) / US2NT(US_PER_SECOND_LL); + // maintain speed at ~0.4V/sec + float preheatDuty = heaterDuty + periodSecs * CJ125_HEATER_PREHEAT_RATE; + cjSetHeater(preheatDuty); + // If we are heating too long, and there's still no result, then something is wrong... + if (nowNt - startHeatingNt > US2NT(US_PER_SECOND_LL) * CJ125_PREHEAT_TIMEOUT) { + cjSetError(CJ125_ERROR_HEATER_MALFUNCTION); + } + cjPrintData(); + prevNt = nowNt; + } + break; + case CJ125_HEAT_UP: + case CJ125_READY: + // use PID for normal heater control + if (nowNt - prevNt >= CJ125_HEATER_CONTROL_PERIOD) { + float duty = heaterPid.getValue(vUr, vUrCal); + heaterPid.showPidStatus(logger, "cj"); + cjSetHeater(duty); + cjPrintData(); + prevNt = nowNt; + } + break; + case CJ125_OVERHEAT: + if (nowNt - prevNt >= CJ125_HEATER_OVERHEAT_PERIOD) { + cjSetError(CJ125_ERROR_OVERHEAT); + prevNt = nowNt; + } + default: + ; + } + + chThdSleepMilliseconds(CJ125_TICK_DELAY); + } + return -1; } +static bool cjCheckConfig(void) { + if (!boardConfiguration->isCJ125Enabled) { + scheduleMsg(logger, "cj125 is disabled. Failed!"); + return false; + } + return true; +} + +static void cjStartCalibration(void) { + if (!cjCheckConfig()) + return; + if (state != CJ125_IDLE) { + // todo: change this later for the normal thread operation (auto pre-heating) + scheduleMsg(logger, "cj125: Cannot start calibration. Please restart the board and make sure that your sensor is not heating"); + return; + } + state = CJ125_CALIBRATION; +} + +static void cjStartTest(void) { + if (!cjCheckConfig()) + return; + state = CJ125_INIT; +} + +#ifdef CJ125_DEBUG +static void cjSetInit1(int v) { + cjWriteRegister(INIT_REG1_WR, v & 0xff); + v = cjReadRegister(INIT_REG1_RD); + scheduleMsg(logger, "cj125 INIT_REG1=0x%02x.", v); +} + +static void cjSetInit2(int v) { + cjWriteRegister(INIT_REG2_WR, v & 0xff); + v = cjReadRegister(INIT_REG2_RD); + scheduleMsg(logger, "cj125 INIT_REG2=0x%02x.", v); +} +#endif /* CJ125_DEBUG */ + float cjGetAfr(DECLARE_ENGINE_PARAMETER_SIGNATURE) { - return 0.0f; + // todo: make configurable sensor LSU type + cj125_sensor_type_e sensorType = CJ125_LSU_42; + + // 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]); + // todo: make configurable stoich ratio + return lambda * CJ125_STOICH_RATIO; } bool cjHasAfrSensor(DECLARE_ENGINE_PARAMETER_SIGNATURE) { - return false; + if (!boardConfiguration->isCJ125Enabled) + return false; + // check if controller is functioning + if (!cjIsWorkingState()) + 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 cjPostState(TunerStudioOutputChannels *tsOutputChannels) { + tsOutputChannels->debugFloatField1 = heaterDuty; + tsOutputChannels->debugFloatField2 = heaterPid.getIntegration(); + tsOutputChannels->debugFloatField3 = heaterPid.getPrevError(); + tsOutputChannels->debugFloatField4 = vUa; + tsOutputChannels->debugFloatField5 = vUr; + tsOutputChannels->debugFloatField6 = vUaCal; + tsOutputChannels->debugFloatField7 = vUrCal; + tsOutputChannels->debugIntField1 = state; + tsOutputChannels->debugIntField2 = diag; +} + +void initCJ125(Logging *sharedLogger) { + logger = sharedLogger; + + if (!boardConfiguration->isCJ125Enabled) + return; + + if (CONFIG(cj125ur) == EFI_ADC_NONE || CONFIG(cj125ua) == EFI_ADC_NONE) { + scheduleMsg(logger, "cj125 init error! cj125ur and cj125ua are required."); + return; + } + + if (boardConfiguration->wboHeaterPin == GPIO_UNASSIGNED) { + scheduleMsg(logger, "cj125 init error! wboHeaterPin is required."); + return; + } + + cjInitPid(); + cjStartSpi(); + cjStartHeaterControl(); + cjStart(); + +#if 1 + state = CJ125_INIT; +#endif + + addConsoleAction("cj125", cjStartTest); + addConsoleAction("cj125_calibrate", cjStartCalibration); +#ifdef CJ125_DEBUG + addConsoleActionF("cj125_heater", cjSetHeater); + addConsoleActionI("cj125_set_init1", cjSetInit1); + addConsoleActionI("cj125_set_init2", cjSetInit2); +#endif /* CJ125_DEBUG */ + + chThdCreateStatic(cjThreadStack, sizeof(cjThreadStack), LOWPRIO, (tfunc_t) cjThread, NULL); } #endif /* EFI_CJ125 */ diff --git a/firmware/hw_layer/sensors/CJ125.h b/firmware/hw_layer/sensors/CJ125.h index e377744c98..194b73881b 100644 --- a/firmware/hw_layer/sensors/CJ125.h +++ b/firmware/hw_layer/sensors/CJ125.h @@ -2,18 +2,108 @@ * @file CJ125.h * * @date: Jul 17, 2016 - * @author Andrey Belomutskiy, (c) 2012-2017 + * @author Andrey Belomutskiy, (c) 2012-2018 */ #ifndef HW_LAYER_SENSORS_CJ125_H_ #define HW_LAYER_SENSORS_CJ125_H_ -#define IDENT_REG_RD 0x48 // Read Identity Register, decimal 72 -#define INIT_REG1_WR 0x56 // Write To Initialization Register 1, decimal 86 -#define CJ125_NORMAL 0x89 // 137 decimal -#define CJ125_CALBRT 0x9D // 157 decimal +// CJ125 SPI Registers +#define IDENT_REG_RD 0x48 // Read Identity Register +#define INIT_REG1_WR 0x56 // Write To Initialization Register 1 +#define INIT_REG2_WR 0x5A // Write To Initialization Register 2 +#define INIT_REG1_RD 0x6C // Read Initialization Register 1 +#define DIAG_REG_RD 0x78 // Read Diagnostics Register +#define INIT_REG2_RD 0x7E // Read Initialization Register 2 -#define CJ125_IDENTT 0x60 +#define CJ125_INIT1_NORMAL_8 0x88 // 0b10001000 (Normal mode, Amplification 8) +#define CJ125_INIT1_NORMAL_17 0x89 // 0b10001001 (Normal mode, Amplification 17) +#define CJ125_INIT1_CALBRT 0x9D // 0b10011101 (Calibration mode, LA=1, RA=1) + +#define CJ125_INIT2_NORMAL 0x00 // 0b00000000, (Normal mode) +#define CJ125_INIT2_DIAG 0x10 // 0b00010000, (Extended diagnostics mode, SET_DIA_Q=1) +#define CJ125_INIT2_RESET 0x40 // 0b01000000, SRESET=1 + +#define CJ125_DIAG_NORM 0xFF // no errors + +#define CJ125_IDENT 0x60 +#define CJ125_IDENT_MASK 0xF8 + +#define CJ125_CALIBRATION_DELAY 1000 // 1 sec +#define CJ125_TICK_DELAY 20 // 20 ms +#define CJ125_IDLE_TICK_DELAY 1000 // 1 sec + +// Heater params for Idle(cold), Preheating and Control stages +// See http://www.waltech.com/wideband-files/boschsensordatasheet.htm +#define CJ125_HEATER_IDLE_RATE 0.15f // for a very cold sensor (presumably), we allow 15% duty max. +#define CJ125_HEATER_PREHEAT_PERIOD 300 // 300 ms +#define CJ125_HEATER_CONTROL_PERIOD 180 // 180 ms +#define CJ125_HEATER_OVERHEAT_PERIOD 500 // 500 ms +#define CJ125_HEATER_PWM_FREQ 100 // 100 Hz +#define CJ125_HEATER_PREHEAT_RATE (0.4f/14.0f) // Assuming that dutycycle=1.0 equals to 14V, and max.allowed heater rate is 0.4V/sec + +#define CJ125_HEATER_LIMITING_VOLTAGE 12.0f // Do not allow more than 90% heating for high battery voltage (>12V). +#define CJ125_HEATER_LIMITING_RATE 0.92f // This prevents sensor overheating. + +#define CJ125_CALIBRATE_NUM_SAMPLES 10 + +#define CJ125_UR_PREHEAT_THR 2.0f // Ur > 2.0 Volts is too cold to control with PID +#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_HEATER_MIN_DUTY 0.1f +#define CJ125_PREHEAT_MIN_DUTY 0.9f + +//#define CJ125_PREHEAT_TIMEOUT 90 // 90 secs +#define CJ125_PREHEAT_TIMEOUT 300 + +#define CJ125_VOLTAGE_TO_16BIT_FACTOR 4096.0f + +#define CJ125_PUMP_SHUNT_RESISTOR 61.9f +#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) + +// Returned if there's no valid measurement +#define CJ125_AFR_NAN 0.0f + +typedef enum { + CJ125_IDLE, + CJ125_INIT, + CJ125_CALIBRATION, + CJ125_PREHEAT, + CJ125_HEAT_UP, + CJ125_READY, + CJ125_OVERHEAT, + CJ125_ERROR, + +} cj125_state_e; + +typedef enum { + CJ125_ERROR_NONE, + CJ125_ERROR_HEATER_MALFUNCTION, + CJ125_ERROR_OVERHEAT, + +} cj125_error_e; + +typedef enum { + CJ125_MODE_NONE, + CJ125_MODE_NORMAL_8, + CJ125_MODE_NORMAL_17, + CJ125_MODE_CALIBRATION, +} cj125_mode_e; + +typedef enum { + CJ125_LSU_42 = 0, + CJ125_LSU_49 = 1, + +} cj125_sensor_type_e; void initCJ125(Logging *shared); @@ -21,9 +111,4 @@ void cjPostState(TunerStudioOutputChannels *tsOutputChannels); float cjGetAfr(DECLARE_ENGINE_PARAMETER_SIGNATURE); bool cjHasAfrSensor(DECLARE_ENGINE_PARAMETER_SIGNATURE); -typedef enum { -CJ125_IDLE = 0 - -} cj125_state_e; - #endif /* HW_LAYER_SENSORS_CJ125_H_ */