diff --git a/firmware/Makefile b/firmware/Makefile index aacde13..91ba981 100644 --- a/firmware/Makefile +++ b/firmware/Makefile @@ -133,6 +133,7 @@ CSRC = $(ALLCSRC) $(BOARDDIR)/board.c \ CPPSRC = $(ALLCPPSRC) \ $(BOARDDIR)/port.cpp \ $(BOARDDIR)/port_shared.cpp \ + wideband_controller.cpp \ shared/flash.cpp \ can.cpp \ can_helper.cpp \ diff --git a/firmware/boards/port.h b/firmware/boards/port.h index a00ce99..1bac0d8 100644 --- a/firmware/boards/port.h +++ b/firmware/boards/port.h @@ -4,15 +4,18 @@ #include "port_shared.h" #include "wideband_config.h" +struct ChannelAnalogResult +{ + float NernstVoltage; + float PumpCurrentVoltage; + /* for dual version - this is voltage on Heater-, switches between zero and Vbatt with heater PWM, + * used for both Vbatt measurement and Heater diagnostic */ + float BatteryVoltage; +}; + struct AnalogResult { - struct { - float NernstVoltage; - float PumpCurrentVoltage; - /* for dual version - this is voltage on Heater-, switches between zero and Vbatt with heater PWM, - * used for both Vbatt measurement and Heater diagnostic */ - float BatteryVoltage; - } ch[AFR_CHANNELS]; + ChannelAnalogResult ch[AFR_CHANNELS]; float VirtualGroundVoltageInt; }; diff --git a/firmware/can.cpp b/firmware/can.cpp index 122f487..366651a 100644 --- a/firmware/can.cpp +++ b/firmware/can.cpp @@ -1,10 +1,10 @@ #include "can.h" #include "hal.h" +#include "wideband_controller.h" #include "fault.h" #include "can_helper.h" #include "heater_control.h" -#include "lambda_conversion.h" #include "sampling.h" #include "pump_dac.h" #include "port.h" @@ -132,8 +132,9 @@ void InitCan() void SendRusefiFormat(uint8_t idx) { + size_t internalIndex = 0; + auto baseAddress = 0x190 + 2 * idx; - auto esr = GetSensorInternalResistance(); { CanTxTyped frame(baseAddress + 0); @@ -141,17 +142,17 @@ void SendRusefiFormat(uint8_t idx) // The same header is imported by the ECU and checked against this data in the frame frame.get().Version = RUSEFI_WIDEBAND_VERSION; - uint16_t lambda = GetLambda() * 10000; + uint16_t lambda = GetController(internalIndex).GetLambda() * 10000; frame.get().Lambda = lambda; - frame.get().TemperatureC = GetSensorTemperature(); + frame.get().TemperatureC = GetController(internalIndex).GetSensorTemperature(); frame.get().Valid = IsRunningClosedLoop() ? 0x01 : 0x00; } { CanTxTyped frame(baseAddress + 1); - frame.get().Esr = esr; - frame.get().NernstDc = GetNernstDc() * 1000; + frame.get().Esr = GetController(internalIndex).GetSensorInternalResistance(); + frame.get().NernstDc = GetController(internalIndex).GetNernstDc() * 1000; frame.get().PumpDuty = GetPumpOutputDuty() * 255; frame.get().Status = GetCurrentFault(); frame.get().HeaterDuty = GetHeaterDuty() * 255; diff --git a/firmware/heater_control.cpp b/firmware/heater_control.cpp index d7c1d1e..531ced8 100644 --- a/firmware/heater_control.cpp +++ b/firmware/heater_control.cpp @@ -4,6 +4,7 @@ #include "ch.h" #include "hal.h" +#include "wideband_controller.h" #include "fault.h" #include "pwm.h" #include "sampling.h" @@ -153,14 +154,14 @@ static void HeaterThread(void*) while (true) { // Read sensor state - float heaterEsr = GetSensorInternalResistance(); + float heaterEsr = GetController().GetSensorInternalResistance(); auto heaterAllowState = GetHeaterAllowed(); // If we haven't heard from rusEFI, use the internally sensed // battery voltage instead of voltage over CAN. float batteryVoltage = heaterAllowState == HeaterAllow::Unknown - ? GetInternalBatteryVoltage() + ? GetController().GetInternalBatteryVoltage() : GetRemoteBatteryVoltage(); // Run the state machine diff --git a/firmware/lambda_conversion.cpp b/firmware/lambda_conversion.cpp index efde3b6..c484ccd 100644 --- a/firmware/lambda_conversion.cpp +++ b/firmware/lambda_conversion.cpp @@ -1,7 +1,6 @@ -#include "lambda_conversion.h" -#include "sampling.h" +#include "wideband_controller.h" -static float GetPhi(float pumpCurrent) +float WidebandController::GetPhi(float pumpCurrent) const { // Maximum lambda ~2 if (pumpCurrent > 1.11f) @@ -22,7 +21,7 @@ static float GetPhi(float pumpCurrent) return gain * pumpCurrent + 0.99559f; } -float GetLambda() +float WidebandController::GetLambda() const { float pumpCurrent = GetPumpNominalCurrent(); diff --git a/firmware/lambda_conversion.h b/firmware/lambda_conversion.h deleted file mode 100644 index 9339cd4..0000000 --- a/firmware/lambda_conversion.h +++ /dev/null @@ -1,3 +0,0 @@ -#pragma once - -float GetLambda(); diff --git a/firmware/pump_control.cpp b/firmware/pump_control.cpp index 9193e29..81de1e3 100644 --- a/firmware/pump_control.cpp +++ b/firmware/pump_control.cpp @@ -1,5 +1,6 @@ #include "pump_control.h" #include "wideband_config.h" +#include "wideband_controller.h" #include "heater_control.h" #include "sampling.h" #include "pump_dac.h" @@ -18,7 +19,7 @@ static void PumpThread(void*) // Only actuate pump when running closed loop! if (IsRunningClosedLoop()) { - float nernstVoltage = GetNernstDc(); + float nernstVoltage = GetController().GetNernstDc(); float result = pumpPid.GetOutput(NERNST_TARGET, nernstVoltage); diff --git a/firmware/sampling.cpp b/firmware/sampling.cpp index e2122ee..5ba8b41 100644 --- a/firmware/sampling.cpp +++ b/firmware/sampling.cpp @@ -5,16 +5,12 @@ #include "hal.h" #include "wideband_config.h" +#include "wideband_controller.h" + #include "port.h" #include "io_pins.h" -// Stored results -static float nernstAc = 0; -static float nernstDc = 0; -static float pumpCurrentSenseVoltage = 0; -static float internalBatteryVoltage = 0; - static const struct inter_point lsu49_r_to_temp[] = { { 80, 1030 }, @@ -46,48 +42,21 @@ static THD_WORKING_AREA(waSamplingThread, 256); static void SamplingThread(void*) { - float r_2 = 0; - float r_3 = 0; + // GD32: Insert delay after enabling ADC + chThdSleepMilliseconds(1); while(true) { - /* TODO: run for all channels */ - int ch = 0; - auto result = AnalogSample(); // Toggle the pin after sampling so that any switching noise occurs while we're doing our math instead of when sampling + // TODO: should this be per-controller, or one toggle pin for the whole board? palTogglePad(NERNST_ESR_DRIVER_PORT, NERNST_ESR_DRIVER_PIN); - float r_1 = result.ch[ch].NernstVoltage; - - // r2_opposite_phase estimates where the previous sample would be had we not been toggling - // AKA the absolute value of the difference between r2_opposite_phase and r2 is the amplitude - // of the AC component on the nernst voltage. We have to pull this trick so as to use the past 3 - // samples to cancel out any slope in the DC (aka actual nernst cell output) from the AC measurement - // See firmware/sampling.png for a drawing of what's going on here - float r2_opposite_phase = (r_1 + r_3) / 2; - - // Compute AC (difference) and DC (average) components - float nernstAcLocal = f_abs(r2_opposite_phase - r_2); - nernstDc = (r2_opposite_phase + r_2) / 2; - - nernstAc = - (1 - ESR_SENSE_ALPHA) * nernstAc + - ESR_SENSE_ALPHA * nernstAcLocal; - - // Exponential moving average (aka first order lpf) - pumpCurrentSenseVoltage = - (1 - PUMP_FILTER_ALPHA) * pumpCurrentSenseVoltage + - PUMP_FILTER_ALPHA * (result.ch[ch].PumpCurrentVoltage - result.VirtualGroundVoltageInt); - - #ifdef BATTERY_INPUT_DIVIDER - internalBatteryVoltage = result.ch[ch].BatteryVoltage; - #endif - - // Shift history over by one - r_3 = r_2; - r_2 = r_1; + for (size_t i = 0; i < AFR_CHANNELS; i++) + { + GetController(i).ProcessSample(result.ch[i], result.VirtualGroundVoltageInt); + } } } @@ -97,12 +66,44 @@ void StartSampling() chThdCreateStatic(waSamplingThread, sizeof(waSamplingThread), NORMALPRIO + 5, SamplingThread, nullptr); } -float GetNernstAc() -{ - return nernstAc; +void WidebandController::ProcessSample(const ChannelAnalogResult& result, float virtualGroundVoltageInt) { + float r_1 = result.NernstVoltage; + + // r2_opposite_phase estimates where the previous sample would be had we not been toggling + // AKA the absolute value of the difference between r2_opposite_phase and r2 is the amplitude + // of the AC component on the nernst voltage. We have to pull this trick so as to use the past 3 + // samples to cancel out any slope in the DC (aka actual nernst cell output) from the AC measurement + // See firmware/sampling.png for a drawing of what's going on here + float r2_opposite_phase = (r_1 + r_3) / 2; + + // Compute AC (difference) and DC (average) components + float nernstAcLocal = f_abs(r2_opposite_phase - r_2); + NernstDc = (r2_opposite_phase + r_2) / 2; + + NernstAc = + (1 - ESR_SENSE_ALPHA) * NernstAc + + ESR_SENSE_ALPHA * nernstAcLocal; + + // Exponential moving average (aka first order lpf) + PumpCurrentSenseVoltage = + (1 - PUMP_FILTER_ALPHA) * PumpCurrentSenseVoltage + + PUMP_FILTER_ALPHA * (result.PumpCurrentVoltage - virtualGroundVoltageInt); + +#ifdef BATTERY_INPUT_DIVIDER + InternalBatteryVoltage = result.BatteryVoltage; +#endif + + // Shift history over by one + r_3 = r_2; + r_2 = r_1; } -float GetSensorInternalResistance() +float WidebandController::GetNernstAc() const +{ + return NernstAc; +} + +float WidebandController::GetSensorInternalResistance() const { // Sensor is the lowside of a divider, top side is 22k, and 3.3v AC pk-pk is injected float totalEsr = ESR_SUPPLY_R / (VCC_VOLTS / GetNernstAc() - 1); @@ -112,7 +113,7 @@ float GetSensorInternalResistance() return totalEsr - VM_RESISTOR_VALUE; } -float GetSensorTemperature() +float WidebandController::GetSensorTemperature() const { float esr = GetSensorInternalResistance(); @@ -124,21 +125,25 @@ float GetSensorTemperature() return interpolate_1d_float(lsu49_r_to_temp, ARRAY_SIZE(lsu49_r_to_temp), esr); } -float GetNernstDc() +float WidebandController::GetNernstDc() const { - return nernstDc; + return NernstDc; } -float GetPumpNominalCurrent() +float WidebandController::GetPumpNominalCurrent() const { // Gain is 10x, then a 61.9 ohm resistor // Effective resistance with the gain is 619 ohms // 1000 is to convert to milliamperes constexpr float ratio = -1000 / (PUMP_CURRENT_SENSE_GAIN * LSU_SENSE_R); - return pumpCurrentSenseVoltage * ratio; + return PumpCurrentSenseVoltage * ratio; } -float GetInternalBatteryVoltage() +float WidebandController::GetInternalBatteryVoltage() const { - return internalBatteryVoltage; +#ifdef BATTERY_INPUT_DIVIDER + return InternalBatteryVoltage; +#else + return 0; +#endif } diff --git a/firmware/sampling.h b/firmware/sampling.h index e376e05..9e36493 100644 --- a/firmware/sampling.h +++ b/firmware/sampling.h @@ -1,12 +1,3 @@ #pragma once void StartSampling(); - -float GetNernstAc(); -float GetSensorInternalResistance(); -float GetSensorTemperature(); -float GetNernstDc(); - -float GetPumpNominalCurrent(); - -float GetInternalBatteryVoltage(); diff --git a/firmware/uart.cpp b/firmware/uart.cpp index 9ec2e45..e81b787 100644 --- a/firmware/uart.cpp +++ b/firmware/uart.cpp @@ -2,7 +2,7 @@ #include "hal.h" #include "chprintf.h" -#include "lambda_conversion.h" +#include "wideband_controller.h" #include "sampling.h" #include "heater_control.h" #include "fault.h" @@ -34,19 +34,19 @@ static void UartThread(void*) { while(true) { - float lambda = GetLambda(); + float lambda = GetController(0).GetLambda(); int lambdaIntPart = lambda; int lambdaThousandths = (lambda - lambdaIntPart) * 1000; - int batteryVoltageMv = GetInternalBatteryVoltage() * 1000; + int batteryVoltageMv = GetController(0).GetInternalBatteryVoltage() * 1000; int duty = GetHeaterDuty() * 100; size_t writeCount = chsnprintf(printBuffer, 200, "%d.%03d\tAC %d mV\tR: %d\tT: %d\tIpump: %d\tVbat: %d\theater: %s (%d)\tfault: %s\r\n", lambdaIntPart, lambdaThousandths, - (int)(GetNernstAc() * 1000.0), - (int)GetSensorInternalResistance(), - (int)GetSensorTemperature(), - (int)(GetPumpNominalCurrent() * 1000), + (int)(GetController(0).GetNernstAc() * 1000.0), + (int)GetController(0).GetSensorInternalResistance(), + (int)GetController(0).GetSensorTemperature(), + (int)(GetController(0).GetPumpNominalCurrent() * 1000), batteryVoltageMv, describeHeaterState(GetHeaterState()), duty, describeFault(GetCurrentFault())); diff --git a/firmware/wideband_controller.cpp b/firmware/wideband_controller.cpp new file mode 100644 index 0000000..0a75852 --- /dev/null +++ b/firmware/wideband_controller.cpp @@ -0,0 +1,9 @@ +#include "wideband_config.h" +#include "wideband_controller.h" + +static WidebandController controllers[AFR_CHANNELS]; + +WidebandController& GetController(size_t idx) +{ + return controllers[idx]; +} diff --git a/firmware/wideband_controller.h b/firmware/wideband_controller.h new file mode 100644 index 0000000..8f5298d --- /dev/null +++ b/firmware/wideband_controller.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +struct ChannelAnalogResult; + +class WidebandController +{ +public: + // SAMPLING: Convert ADC information in to O2 sensor information. + void ProcessSample(const ChannelAnalogResult& result, float virtualGroundVoltageInt); + float GetNernstDc() const; + float GetNernstAc() const; + float GetSensorInternalResistance() const; + float GetSensorTemperature() const; + float GetPumpNominalCurrent() const; + float GetInternalBatteryVoltage() const; + + // LAMBDA CONVERSION: Convert to a useful lambda value. + float GetLambda() const; + float GetPhi(float pumpCurrent) const; +private: + float NernstAc = 0; + float NernstDc = 0; + float PumpCurrentSenseVoltage = 0; + +#ifdef BATTERY_INPUT_DIVIDER + float InternalBatteryVoltage = 0; +#endif + + float r_2 = 0; + float r_3 = 0; +}; + +WidebandController& GetController(size_t idx = 0);