diff --git a/firmware/auxout.cpp b/firmware/auxout.cpp index a1478ba..74fa709 100644 --- a/firmware/auxout.cpp +++ b/firmware/auxout.cpp @@ -53,7 +53,10 @@ void AuxOutThread(void*) { while(1) { - SetAuxDac(0, GetLambda()); + for (int ch = 0; ch < AFR_CHANNELS; ch++) + { + SetAuxDac(ch, GetLambda(ch)); + } chThdSleepMilliseconds(10); } diff --git a/firmware/boards/f0_module/io/io_pins.h b/firmware/boards/f0_module/io/io_pins.h index 4380d87..8aafb5d 100644 --- a/firmware/boards/f0_module/io/io_pins.h +++ b/firmware/boards/f0_module/io/io_pins.h @@ -11,11 +11,11 @@ // PA7 #define HEATER_PWM_DEVICE PWMD1 -#define HEATER_PWM_CHANNEL 0 +#define HEATER_PWM_CHANNEL_0 0 // PA6 #define PUMP_DAC_PWM_DEVICE PWMD3 -#define PUMP_DAC_PWM_CHANNEL 0 +#define PUMP_DAC_PWM_CHANNEL_0 0 #define ID_SEL1_PORT GPIOB #define ID_SEL1_PIN 1 diff --git a/firmware/boards/f1_dual/io/io_pins.h b/firmware/boards/f1_dual/io/io_pins.h index d7520be..0699afa 100644 --- a/firmware/boards/f1_dual/io/io_pins.h +++ b/firmware/boards/f1_dual/io/io_pins.h @@ -31,13 +31,12 @@ // L_heater_pwm - PB7 TIM4_CH2 #define HEATER_PWM_DEVICE PWMD4 -#define HEATER_PWM_CHANNEL 1 +#define HEATER_PWM_CHANNEL_0 1 #define L_HEATER_PORT GPIOB #define L_HEATER_PIN 7 -// B_heater_pwm - PB6 TIM4_CH1 -#define R_HEATER_PWM_DEVICE PWMD4 -#define R_HEATER_PWM_CHANNEL 0 +// R_heater_pwm - PB6 TIM4_CH1 +#define HEATER_PWM_CHANNEL_1 0 #define R_HEATER_PORT GPIOB #define R_HEATER_PIN 6 @@ -51,7 +50,8 @@ // PA1 TIM2_CH2 #define PUMP_DAC_PWM_DEVICE PWMD2 -#define PUMP_DAC_PWM_CHANNEL 1 +#define PUMP_DAC_PWM_CHANNEL_0 2 /* left */ +#define PUMP_DAC_PWM_CHANNEL_1 1 /* right */ // TIM1 - DAC for AUX outputs #define AUXOUT_DAC_PWM_DEVICE PWMD1 diff --git a/firmware/boards/f1_rev2/io/io_pins.h b/firmware/boards/f1_rev2/io/io_pins.h index a10fe4d..8006440 100644 --- a/firmware/boards/f1_rev2/io/io_pins.h +++ b/firmware/boards/f1_rev2/io/io_pins.h @@ -31,11 +31,11 @@ // PB6 TIM4_CH1 #define HEATER_PWM_DEVICE PWMD4 -#define HEATER_PWM_CHANNEL 0 +#define HEATER_PWM_CHANNEL_0 0 // PA1 TIM2_CH2 #define PUMP_DAC_PWM_DEVICE PWMD2 -#define PUMP_DAC_PWM_CHANNEL 1 +#define PUMP_DAC_PWM_CHANNEL_0 1 // TIM1 - DAC for AUX outputs #define AUXOUT_DAC_PWM_DEVICE PWMD1 diff --git a/firmware/boards/f1_rev3/io/io_pins.h b/firmware/boards/f1_rev3/io/io_pins.h index a855598..d72b5d7 100644 --- a/firmware/boards/f1_rev3/io/io_pins.h +++ b/firmware/boards/f1_rev3/io/io_pins.h @@ -31,11 +31,11 @@ // PB6 TIM4_CH1 #define HEATER_PWM_DEVICE PWMD4 -#define HEATER_PWM_CHANNEL 0 +#define HEATER_PWM_CHANNEL_0 0 // PA1 TIM2_CH2 #define PUMP_DAC_PWM_DEVICE PWMD2 -#define PUMP_DAC_PWM_CHANNEL 1 +#define PUMP_DAC_PWM_CHANNEL_0 1 // DAC for AUX outputs #define AUXOUT_DAC_DEVICE DACD1 diff --git a/firmware/can.cpp b/firmware/can.cpp index 272718f..8d9410d 100644 --- a/firmware/can.cpp +++ b/firmware/can.cpp @@ -131,7 +131,8 @@ void InitCan() void SendRusefiFormat(uint8_t idx) { auto baseAddress = WB_DATA_BASE_ADDR + 2 * idx; - auto esr = GetSensorInternalResistance(); + /* TODO: */ + int ch = 0; { CanTxTyped frame(baseAddress + 0); @@ -139,19 +140,21 @@ 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 = GetLambda(ch) * 10000; frame.get().Lambda = lambda; - frame.get().TemperatureC = GetSensorTemperature(); - frame.get().Valid = IsRunningClosedLoop() ? 0x01 : 0x00; + frame.get().TemperatureC = GetSensorTemperature(ch); + frame.get().Valid = IsRunningClosedLoop(ch) ? 0x01 : 0x00; } { + auto esr = GetSensorInternalResistance(ch); + CanTxTyped frame(baseAddress + 1); frame.get().Esr = esr; - frame.get().NernstDc = GetNernstDc() * 1000; - frame.get().PumpDuty = GetPumpOutputDuty() * 255; - frame.get().Status = GetCurrentFault(); - frame.get().HeaterDuty = GetHeaterDuty() * 255; + frame.get().NernstDc = GetNernstDc(ch) * 1000; + frame.get().PumpDuty = GetPumpOutputDuty(ch) * 255; + frame.get().Status = GetCurrentFault(ch); + frame.get().HeaterDuty = GetHeaterDuty(ch) * 255; } } diff --git a/firmware/fault.cpp b/firmware/fault.cpp index 6d067a1..96eef8c 100644 --- a/firmware/fault.cpp +++ b/firmware/fault.cpp @@ -4,7 +4,7 @@ using namespace wbo; static Fault currentFault = Fault::None; -void SetFault(Fault fault) +void SetFault(int ch, Fault fault) { currentFault = fault; } @@ -14,7 +14,7 @@ bool HasFault() return currentFault != Fault::None; } -Fault GetCurrentFault() +Fault GetCurrentFault(int ch) { return currentFault; } diff --git a/firmware/fault.h b/firmware/fault.h index d19f8a5..00c5a2d 100644 --- a/firmware/fault.h +++ b/firmware/fault.h @@ -4,6 +4,6 @@ #include "../for_rusefi/wideband_can.h" -void SetFault(wbo::Fault fault); +void SetFault(int ch, wbo::Fault fault); bool HasFault(); -wbo::Fault GetCurrentFault(); +wbo::Fault GetCurrentFault(int ch); diff --git a/firmware/heater_control.cpp b/firmware/heater_control.cpp index f21c4af..619b13c 100644 --- a/firmware/heater_control.cpp +++ b/firmware/heater_control.cpp @@ -13,15 +13,74 @@ using namespace wbo; // 400khz / 1024 = 390hz PWM -static Pwm heaterPwm(HEATER_PWM_DEVICE, HEATER_PWM_CHANNEL, 400'000, 1024); +static Pwm heaterPwm(HEATER_PWM_DEVICE); +PWMConfig heaterPwmConfig = { + 400'000, + 1024, + nullptr, + { + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr} + }, + 0, + 0, +#if STM32_PWM_USE_ADVANCED + 0 +#endif +}; static constexpr int preheatTimeCounter = HEATER_PREHEAT_TIME / HEATER_CONTROL_PERIOD; static constexpr int batteryStabTimeCounter = HEATER_BATTERY_STAB_TIME / HEATER_CONTROL_PERIOD; -static int timeCounter = preheatTimeCounter; -static int batteryStabTime = batteryStabTimeCounter; -static float rampVoltage = 0; -static HeaterState GetNextState(HeaterState state, HeaterAllow heaterAllowState, float batteryVoltage, float sensorEsr) +struct heater_state { + Pid heaterPid; + int timeCounter; + int batteryStabTime; + float rampVoltage; + HeaterState heaterState; + uint8_t ch; + uint8_t pwm_ch; +}; + +static struct heater_state state[AFR_CHANNELS] = +{ + { + .heaterPid = Pid( + 0.3f, // kP + 0.3f, // kI + 0.01f, // kD + 3.0f, // Integrator clamp (volts) + HEATER_CONTROL_PERIOD + ), + .timeCounter = preheatTimeCounter, + .batteryStabTime = batteryStabTimeCounter, + .rampVoltage = 0, + .heaterState = HeaterState::Preheat, + .ch = 0, + .pwm_ch = HEATER_PWM_CHANNEL_0, + }, +#if (AFR_CHANNELS > 1) + { + .heaterPid = Pid( + 0.3f, // kP + 0.3f, // kI + 0.01f, // kD + 3.0f, // Integrator clamp (volts) + HEATER_CONTROL_PERIOD + ), + .timeCounter = preheatTimeCounter, + .batteryStabTime = batteryStabTimeCounter, + .rampVoltage = 0, + .heaterState = HeaterState::Preheat, + .ch = 1, + .pwm_ch = HEATER_PWM_CHANNEL_1, + }, +#endif +}; + +static HeaterState GetNextState(struct heater_state &s, HeaterAllow heaterAllowState, float batteryVoltage, float sensorEsr) { bool heaterAllowed = heaterAllowState == HeaterAllow::Allowed; @@ -31,37 +90,37 @@ static HeaterState GetNextState(HeaterState state, HeaterAllow heaterAllowState, // measured voltage too low to auto-start heating if (batteryVoltage < HEATER_BATTETY_OFF_VOLTAGE) { - batteryStabTime = batteryStabTimeCounter; + s.batteryStabTime = batteryStabTimeCounter; } // measured voltage is high enougth to auto-start heating, wait some time to stabilize - if ((batteryVoltage > HEATER_BATTERY_ON_VOLTAGE) && (batteryStabTime > 0)) + if ((batteryVoltage > HEATER_BATTERY_ON_VOLTAGE) && (s.batteryStabTime > 0)) { - batteryStabTime--; + s.batteryStabTime--; } - heaterAllowed = batteryStabTime == 0; + heaterAllowed = s.batteryStabTime == 0; } if (!heaterAllowed) { // ECU hasn't allowed preheat yet, reset timer, and force preheat state - timeCounter = preheatTimeCounter; + s.timeCounter = preheatTimeCounter; return HeaterState::Preheat; } - switch (state) + switch (s.heaterState) { case HeaterState::Preheat: - timeCounter--; + s.timeCounter--; // If preheat timeout, or sensor is already hot (engine running?) - if (timeCounter <= 0 || sensorEsr < HEATER_CLOSED_LOOP_THRESHOLD_ESR) + if (s.timeCounter <= 0 || sensorEsr < HEATER_CLOSED_LOOP_THRESHOLD_ESR) { // If enough time has elapsed, start the ramp // Start the ramp at 4 volts - rampVoltage = 4; + s.rampVoltage = 4; // Next phase times out at 15 seconds - timeCounter = HEATER_WARMUP_TIMEOUT / HEATER_CONTROL_PERIOD; + s.timeCounter = HEATER_WARMUP_TIMEOUT / HEATER_CONTROL_PERIOD; return HeaterState::WarmupRamp; } @@ -73,25 +132,25 @@ static HeaterState GetNextState(HeaterState state, HeaterAllow heaterAllowState, { return HeaterState::ClosedLoop; } - else if (timeCounter == 0) + else if (s.timeCounter == 0) { - SetFault(Fault::SensorDidntHeat); + SetFault(s.ch, Fault::SensorDidntHeat); return HeaterState::Stopped; } - timeCounter--; + s.timeCounter--; break; case HeaterState::ClosedLoop: // Check that the sensor's ESR is acceptable for normal operation if (sensorEsr < HEATER_OVERHEAT_ESR) { - SetFault(Fault::SensorOverheat); + SetFault(s.ch, Fault::SensorOverheat); return HeaterState::Stopped; } else if (sensorEsr > HEATER_UNDERHEAT_ESR) { - SetFault(Fault::SensorUnderheat); + SetFault(s.ch, Fault::SensorUnderheat); return HeaterState::Stopped; } @@ -99,38 +158,30 @@ static HeaterState GetNextState(HeaterState state, HeaterAllow heaterAllowState, case HeaterState::Stopped: break; } - return state; + return s.heaterState; } -static Pid heaterPid( - 0.3f, // kP - 0.3f, // kI - 0.01f, // kD - 3.0f, // Integrator clamp (volts) - HEATER_CONTROL_PERIOD -); - -static float GetVoltageForState(HeaterState state, float heaterEsr) +static float GetVoltageForState(struct heater_state &s, float heaterEsr) { - switch (state) + switch (s.heaterState) { case HeaterState::Preheat: // Max allowed during condensation phase (preheat) is 2v return 1.5f; case HeaterState::WarmupRamp: - if (rampVoltage < 10) + if (s.rampVoltage < 10) { // 0.3 volt per second, divided by battery voltage and update rate constexpr float rampRateVoltPerSecond = 0.3f; constexpr float heaterFrequency = 1000.0f / HEATER_CONTROL_PERIOD; - rampVoltage += (rampRateVoltPerSecond / heaterFrequency); + s.rampVoltage += (rampRateVoltPerSecond / heaterFrequency); } - return rampVoltage; + return s.rampVoltage; case HeaterState::ClosedLoop: // "nominal" heater voltage is 7.5v, so apply correction around that point (instead of relying on integrator so much) // Negated because lower resistance -> hotter - return 7.5f - heaterPid.GetOutput(HEATER_TARGET_ESR, heaterEsr); + return 7.5f - s.heaterPid.GetOutput(HEATER_TARGET_ESR, heaterEsr); case HeaterState::Stopped: // Something has gone wrong, turn off the heater. return 0; @@ -140,57 +191,60 @@ static float GetVoltageForState(HeaterState state, float heaterEsr) return 0; } -static HeaterState state = HeaterState::Preheat; - - static THD_WORKING_AREA(waHeaterThread, 256); static void HeaterThread(void*) { + int ch; + // Wait for temperature sensing to stabilize so we don't // immediately think we overshot the target temperature chThdSleepMilliseconds(1000); while (true) { - // Read sensor state - float heaterEsr = 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() - : GetRemoteBatteryVoltage(); + for (ch = 0; ch < AFR_CHANNELS; ch++) { + heater_state &s = state[ch]; - // Run the state machine - state = GetNextState(state, heaterAllowState, batteryVoltage, heaterEsr); - float heaterVoltage = GetVoltageForState(state, heaterEsr); + // Read sensor state + float heaterEsr = GetSensorInternalResistance(ch); - // Limit to 11 volts - if (heaterVoltage > 11) { - heaterVoltage = 11; - } + // If we haven't heard from rusEFI, use the internally sensed + // battery voltage instead of voltage over CAN. + float batteryVoltage = heaterAllowState == HeaterAllow::Unknown + ? GetInternalBatteryVoltage(ch) + : GetRemoteBatteryVoltage(); - // duty = (V_eff / V_batt) ^ 2 - float voltageRatio = heaterVoltage / batteryVoltage; - float duty = voltageRatio * voltageRatio; + // Run the state machine + s.heaterState = GetNextState(s, heaterAllowState, batteryVoltage, heaterEsr); + float heaterVoltage = GetVoltageForState(s, heaterEsr); - #ifdef HEATER_MAX_DUTY - if (duty > HEATER_MAX_DUTY) { - duty = HEATER_MAX_DUTY; - } - #endif + // Limit to 11 volts + if (heaterVoltage > 11) { + heaterVoltage = 11; + } - if (batteryVoltage < 23) - { - // Pipe the output to the heater driver - heaterPwm.SetDuty(duty); - } - else - { - // Overvoltage protection - sensor not rated for PWM above 24v - heaterPwm.SetDuty(0); + // duty = (V_eff / V_batt) ^ 2 + float voltageRatio = heaterVoltage / batteryVoltage; + float duty = voltageRatio * voltageRatio; + + #ifdef HEATER_MAX_DUTY + if (duty > HEATER_MAX_DUTY) { + duty = HEATER_MAX_DUTY; + } + #endif + + if (batteryVoltage < 23) + { + // Pipe the output to the heater driver + heaterPwm.SetDuty(s.pwm_ch, duty); + } + else + { + // Overvoltage protection - sensor not rated for PWM above 24v + heaterPwm.SetDuty(s.pwm_ch, 0); + } } // Loop at ~20hz @@ -200,25 +254,28 @@ static void HeaterThread(void*) void StartHeaterControl() { - heaterPwm.Start(); - heaterPwm.SetDuty(0); + heaterPwm.Start(heaterPwmConfig); + heaterPwm.SetDuty(state[0].pwm_ch, 0); +#if (AFR_CHANNELS > 1) + heaterPwm.SetDuty(state[1].pwm_ch, 0); +#endif chThdCreateStatic(waHeaterThread, sizeof(waHeaterThread), NORMALPRIO + 1, HeaterThread, nullptr); } -bool IsRunningClosedLoop() +bool IsRunningClosedLoop(int ch) { - return state == HeaterState::ClosedLoop; + return state[ch].heaterState == HeaterState::ClosedLoop; } -float GetHeaterDuty() +float GetHeaterDuty(int ch) { - return heaterPwm.GetLastDuty(); + return heaterPwm.GetLastDuty(state[ch].pwm_ch); } -HeaterState GetHeaterState() +HeaterState GetHeaterState(int ch) { - return state; + return state[ch].heaterState; } const char* describeHeaterState(HeaterState state) diff --git a/firmware/heater_control.h b/firmware/heater_control.h index 72fb4bb..e67f023 100644 --- a/firmware/heater_control.h +++ b/firmware/heater_control.h @@ -11,7 +11,7 @@ enum class HeaterState }; void StartHeaterControl(); -bool IsRunningClosedLoop(); -float GetHeaterDuty(); -HeaterState GetHeaterState(); +bool IsRunningClosedLoop(int ch); +float GetHeaterDuty(int ch); +HeaterState GetHeaterState(int ch); const char* describeHeaterState(HeaterState state); diff --git a/firmware/lambda_conversion.cpp b/firmware/lambda_conversion.cpp index efde3b6..70a5ba5 100644 --- a/firmware/lambda_conversion.cpp +++ b/firmware/lambda_conversion.cpp @@ -22,9 +22,9 @@ static float GetPhi(float pumpCurrent) return gain * pumpCurrent + 0.99559f; } -float GetLambda() +float GetLambda(int ch) { - float pumpCurrent = GetPumpNominalCurrent(); + float pumpCurrent = GetPumpNominalCurrent(ch); // Lambda is reciprocal of phi return 1 / GetPhi(pumpCurrent); diff --git a/firmware/lambda_conversion.h b/firmware/lambda_conversion.h index 9339cd4..9ed8215 100644 --- a/firmware/lambda_conversion.h +++ b/firmware/lambda_conversion.h @@ -1,3 +1,3 @@ #pragma once -float GetLambda(); +float GetLambda(int ch); diff --git a/firmware/livedata.cpp b/firmware/livedata.cpp index d67f843..4d02027 100644 --- a/firmware/livedata.cpp +++ b/firmware/livedata.cpp @@ -1,3 +1,4 @@ +#include "wideband_config.h" #include "livedata.h" #include "lambda_conversion.h" @@ -9,18 +10,23 @@ #include static livedata_common_s livedata_common; -static livedata_afr_s livedata_afr; +static livedata_afr_s livedata_afr[AFR_CHANNELS]; void SamplingUpdateLiveData() { - livedata_afr.afr = GetLambda(); - livedata_afr.temperature = GetSensorTemperature(); - livedata_afr.nernstVoltage = GetNernstDc(); - livedata_afr.pumpCurrentTarget = GetPumpCurrent(); - livedata_afr.pumpCurrentMeasured = GetPumpNominalCurrent(); - livedata_afr.heaterDuty = GetHeaterDuty(); + for (int ch = 0; ch < AFR_CHANNELS; ch++) + { + volatile struct livedata_afr_s *data = &livedata_afr[ch]; - livedata_common.vbatt = GetInternalBatteryVoltage(); + data->afr = GetLambda(ch); + data->temperature = GetSensorTemperature(ch); + data->nernstVoltage = GetNernstDc(ch); + data->pumpCurrentTarget = GetPumpCurrent(ch); + data->pumpCurrentMeasured = GetPumpNominalCurrent(ch); + data->heaterDuty = GetHeaterDuty(ch); + } + + livedata_common.vbatt = GetInternalBatteryVoltage(0); } const livedata_common_s * getCommonLiveDataStructAddr() @@ -28,14 +34,16 @@ const livedata_common_s * getCommonLiveDataStructAddr() return &livedata_common; } -const livedata_afr_s * getAfrLiveDataStructAddr() +const struct livedata_afr_s * getAfrLiveDataStructAddr(const int ch) { - return &livedata_afr; + if (ch < AFR_CHANNELS) + return &livedata_afr[ch]; + return NULL; } static const FragmentEntry fragments[] = { getCommonLiveDataStructAddr(), - getAfrLiveDataStructAddr(), + getAfrLiveDataStructAddr(0), }; FragmentList getFragments() { diff --git a/firmware/livedata.h b/firmware/livedata.h index 28d08e1..ca06aab 100644 --- a/firmware/livedata.h +++ b/firmware/livedata.h @@ -2,6 +2,8 @@ #include +#include "wideband_config.h" + /* +0 offset */ struct livedata_common_s { union { @@ -33,4 +35,4 @@ void SamplingUpdateLiveData(); /* access functions */ const struct livedata_common_s * getCommonLiveDataStructAddr(); -const struct livedata_afr_s * getAfrLiveDataStructAddr(); +const struct livedata_afr_s * getAfrLiveDataStructAddr(const int ch); diff --git a/firmware/main.cpp b/firmware/main.cpp index 1c585a7..aecca32 100644 --- a/firmware/main.cpp +++ b/firmware/main.cpp @@ -42,7 +42,9 @@ int main() { while(true) { - auto fault = GetCurrentFault(); + /* TODO: show error for all AFR channels */ + /* TODO: show EGT errors */ + auto fault = GetCurrentFault(0); if (fault == Fault::None) { @@ -53,7 +55,7 @@ int main() { palTogglePad(LED_GREEN_PORT, LED_GREEN_PIN); // Slow blink if closed loop, fast if not - chThdSleepMilliseconds(IsRunningClosedLoop() ? 700 : 50); + chThdSleepMilliseconds(IsRunningClosedLoop(0) ? 700 : 50); } else { diff --git a/firmware/pump_control.cpp b/firmware/pump_control.cpp index 9193e29..b78b3bc 100644 --- a/firmware/pump_control.cpp +++ b/firmware/pump_control.cpp @@ -7,28 +7,46 @@ #include "ch.h" -// Bosch CJ125 is somewhere VERY ROUGHLY like 200-400A/(v*s) integrator gain -static Pid pumpPid(50.0f, 10000.0f, 0, 10, 2); +struct pump_control_state { + Pid pumpPid; +}; + +static struct pump_control_state state[AFR_CHANNELS] = +{ + { + Pid(50.0f, 10000.0f, 0.0f, 10.0f, 2), + }, +#if (AFR_CHANNELS > 1) + { + Pid(50.0f, 10000.0f, 0.0f, 10.0f, 2), + } +#endif +}; static THD_WORKING_AREA(waPumpThread, 256); static void PumpThread(void*) { while(true) { - // Only actuate pump when running closed loop! - if (IsRunningClosedLoop()) + for (int ch = 0; ch < AFR_CHANNELS; ch++) { - float nernstVoltage = GetNernstDc(); + pump_control_state &s = state[ch]; - float result = pumpPid.GetOutput(NERNST_TARGET, nernstVoltage); + // Only actuate pump when running closed loop! + if (IsRunningClosedLoop(ch)) + { + float nernstVoltage = GetNernstDc(ch); - // result is in mA - SetPumpCurrentTarget(result * 1000); - } - else - { - // Otherwise set zero pump current to avoid damaging the sensor - SetPumpCurrentTarget(0); + float result = s.pumpPid.GetOutput(NERNST_TARGET, nernstVoltage); + + // result is in mA + SetPumpCurrentTarget(ch, result * 1000); + } + else + { + // Otherwise set zero pump current to avoid damaging the sensor + SetPumpCurrentTarget(ch, 0); + } } // Run at 500hz diff --git a/firmware/pump_dac.cpp b/firmware/pump_dac.cpp index 787cb0e..69d37cb 100644 --- a/firmware/pump_dac.cpp +++ b/firmware/pump_dac.cpp @@ -7,28 +7,59 @@ #include "hal.h" // 48MHz / 1024 = 46.8khz PWM -static Pwm pumpDac(PUMP_DAC_PWM_DEVICE, PUMP_DAC_PWM_CHANNEL, 48'000'000, 1024); +PWMConfig pumpDacConfig = { + 48'000'000, + 1024, + nullptr, + { + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr}, + {PWM_OUTPUT_ACTIVE_HIGH, nullptr} + }, + 0, + 0, +#if STM32_PWM_USE_ADVANCED + 0 +#endif +}; -static int32_t curIpump; +static Pwm pumpDac(PUMP_DAC_PWM_DEVICE); + +struct pump_dac_state { + int32_t curIpump; +}; + +static const uint8_t pumpDacPwmCh[] = { + PUMP_DAC_PWM_CHANNEL_0, +#if (AFR_CHANNELS > 1) + PUMP_DAC_PWM_CHANNEL_1, +#endif +}; + +static struct pump_dac_state state[AFR_CHANNELS]; void InitPumpDac() { - pumpDac.Start(); + pumpDac.Start(pumpDacConfig); - // Set zero current to start - sensor can be damaged if current flowing - // while warming up - SetPumpCurrentTarget(0); + for (int ch = 0; ch < AFR_CHANNELS; ch++) + { + // Set zero current to start - sensor can be damaged if current flowing + // while warming up + SetPumpCurrentTarget(ch, 0); + } } -void SetPumpCurrentTarget(int32_t microampere) +void SetPumpCurrentTarget(int ch, int32_t microampere) { // Don't allow pump current when the sensor isn't hot - if (!IsRunningClosedLoop()) + if (!IsRunningClosedLoop(ch)) { microampere = 0; } - curIpump = microampere; + state[ch].curIpump = microampere; // 47 ohm resistor // 0.147 gain @@ -38,15 +69,15 @@ void SetPumpCurrentTarget(int32_t microampere) // offset by half vcc volts += HALF_VCC; - pumpDac.SetDuty(volts / VCC_VOLTS); + pumpDac.SetDuty(pumpDacPwmCh[ch], volts / VCC_VOLTS); } -float GetPumpOutputDuty() +float GetPumpOutputDuty(int ch) { - return pumpDac.GetLastDuty(); + return pumpDac.GetLastDuty(ch); } -float GetPumpCurrent() +float GetPumpCurrent(int ch) { - return (float)curIpump / 1000.0; + return (float)state[ch].curIpump / 1000.0; } diff --git a/firmware/pump_dac.h b/firmware/pump_dac.h index 3f30b8c..ed19f58 100644 --- a/firmware/pump_dac.h +++ b/firmware/pump_dac.h @@ -3,6 +3,6 @@ #include void InitPumpDac(); -void SetPumpCurrentTarget(int32_t microamperes); -float GetPumpOutputDuty(); -float GetPumpCurrent(); +void SetPumpCurrentTarget(int ch, int32_t microamperes); +float GetPumpOutputDuty(int ch); +float GetPumpCurrent(int ch); diff --git a/firmware/pwm.cpp b/firmware/pwm.cpp index fa418ce..1c96450 100644 --- a/firmware/pwm.cpp +++ b/firmware/pwm.cpp @@ -2,17 +2,8 @@ #include -Pwm::Pwm(PWMDriver& driver, uint8_t channel, uint32_t counterFrequency, uint32_t counterPeriod) - : m_driver(&driver) - , m_channel(channel) - , m_counterFrequency(counterFrequency) - , m_counterPeriod(counterPeriod) -{ -} - Pwm::Pwm(PWMDriver& driver) : m_driver(&driver) - , m_channel(0) , m_counterFrequency(0) , m_counterPeriod(0) { @@ -50,17 +41,13 @@ void Pwm::Start(PWMConfig& config) void Pwm::SetDuty(int channel, float duty) { auto dutyFloat = clampF(0, duty, 1); - m_dutyFloat = dutyFloat; + m_dutyFloat[channel] = dutyFloat; pwmcnt_t highTime = m_counterPeriod * dutyFloat; - pwm_lld_enable_channel(m_driver, channel, highTime); + pwmEnableChannel(m_driver, channel, highTime); } -void Pwm::SetDuty(float duty) { - SetDuty(m_channel, duty); -} - -float Pwm::GetLastDuty() const +float Pwm::GetLastDuty(int channel) { - return m_dutyFloat; + return m_dutyFloat[channel]; } diff --git a/firmware/pwm.h b/firmware/pwm.h index 759452f..00f83ad 100644 --- a/firmware/pwm.h +++ b/firmware/pwm.h @@ -10,19 +10,17 @@ struct PWMDriver; class Pwm { public: - Pwm(PWMDriver& driver, uint8_t channel, uint32_t counterFrequency, uint32_t counterPeriod); Pwm(PWMDriver& driver); void Start(); void Start(PWMConfig& config); - void SetDuty(float duty); void SetDuty(int channel, float duty); - float GetLastDuty() const; + float GetLastDuty(int channel); private: PWMDriver* const m_driver; - const uint8_t m_channel; + //const uint8_t m_channel; /* const */ uint32_t m_counterFrequency; /* const */ uint16_t m_counterPeriod; - float m_dutyFloat; + float m_dutyFloat[4]; }; diff --git a/firmware/sampling.cpp b/firmware/sampling.cpp index 0368ff7..11e7e8e 100644 --- a/firmware/sampling.cpp +++ b/firmware/sampling.cpp @@ -12,10 +12,14 @@ #include // Stored results -static float nernstAc = 0; -static float nernstDc = 0; -static float pumpCurrentSenseVoltage = 0; -static float internalBatteryVoltage = 0; +struct measure_results { + float nernstAc; + float nernstDc; + float pumpCurrentSenseVoltage; + float internalBatteryVoltage; +}; + +static struct measure_results results[AFR_CHANNELS]; // Last point is approximated by the greatest measurable sensor resistance static const float lsu49TempBins[] = { 80, 150, 200, 250, 300, 350, 400, 450, 550, 650, 800, 1000, 1200, 2500, 5000 }; @@ -30,51 +34,51 @@ static THD_WORKING_AREA(waSamplingThread, 256); static void SamplingThread(void*) { - float r_2 = 0; - float r_3 = 0; + float r_2[AFR_CHANNELS] = {0}; + float r_3[AFR_CHANNELS] = {0}; /* GD32: Insert 20us delay after ADC enable */ 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 palTogglePad(NERNST_ESR_DRIVER_PORT, NERNST_ESR_DRIVER_PIN); - float r_1 = result.ch[ch].NernstVoltage; + for (int ch = 0; ch < AFR_CHANNELS; ch++) { + measure_results &res = results[ch]; + 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; + // 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[ch]) / 2; - // Compute AC (difference) and DC (average) components - float nernstAcLocal = f_abs(r2_opposite_phase - r_2); - nernstDc = (r2_opposite_phase + r_2) / 2; + // Compute AC (difference) and DC (average) components + float nernstAcLocal = f_abs(r2_opposite_phase - r_2[ch]); + res.nernstDc = (r2_opposite_phase + r_2[ch]) / 2; - nernstAc = - (1 - ESR_SENSE_ALPHA) * nernstAc + - ESR_SENSE_ALPHA * nernstAcLocal; + res.nernstAc = + (1 - ESR_SENSE_ALPHA) * res.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); + // Exponential moving average (aka first order lpf) + res.pumpCurrentSenseVoltage = + (1 - PUMP_FILTER_ALPHA) * res.pumpCurrentSenseVoltage + + PUMP_FILTER_ALPHA * (result.ch[ch].PumpCurrentVoltage - result.VirtualGroundVoltageInt); - #ifdef BATTERY_INPUT_DIVIDER - internalBatteryVoltage = result.ch[ch].BatteryVoltage; - #endif + #ifdef BATTERY_INPUT_DIVIDER + res.internalBatteryVoltage = result.ch[ch].BatteryVoltage; + #endif - // Shift history over by one - r_3 = r_2; - r_2 = r_1; + // Shift history over by one + r_3[ch] = r_2[ch]; + r_2[ch] = r_1; + } #if defined(TS_ENABLED) /* tunerstudio */ @@ -89,24 +93,24 @@ void StartSampling() chThdCreateStatic(waSamplingThread, sizeof(waSamplingThread), NORMALPRIO + 5, SamplingThread, nullptr); } -float GetNernstAc() +float GetNernstAc(int ch) { - return nernstAc; + return results[ch].nernstAc; } -float GetSensorInternalResistance() +float GetSensorInternalResistance(int ch) { // 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); + float totalEsr = ESR_SUPPLY_R / (VCC_VOLTS / GetNernstAc(ch) - 1); // There is a resistor between the opamp and Vm sensor pin. Remove the effect of that // resistor so that the remainder is only the ESR of the sensor itself return totalEsr - VM_RESISTOR_VALUE; } -float GetSensorTemperature() +float GetSensorTemperature(int ch) { - float esr = GetSensorInternalResistance(); + float esr = GetSensorInternalResistance(ch); if (esr > 5000) { @@ -116,21 +120,24 @@ float GetSensorTemperature() return interpolate2d(esr, lsu49TempBins, lsu49TempValues); } -float GetNernstDc() +float GetNernstDc(int ch) { - return nernstDc; + return results[ch].nernstDc; } -float GetPumpNominalCurrent() +float GetPumpNominalCurrent(int ch) { // 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 results[ch].pumpCurrentSenseVoltage * ratio; } -float GetInternalBatteryVoltage() +float GetInternalBatteryVoltage(int ch) { - return internalBatteryVoltage; + // Dual HW can measure heater voltage for each channel + // by measuring voltage on Heater- while FET is off + // TODO: rename function? + return results[ch].internalBatteryVoltage; } diff --git a/firmware/sampling.h b/firmware/sampling.h index e376e05..c1415e0 100644 --- a/firmware/sampling.h +++ b/firmware/sampling.h @@ -2,11 +2,9 @@ void StartSampling(); -float GetNernstAc(); -float GetSensorInternalResistance(); -float GetSensorTemperature(); -float GetNernstDc(); - -float GetPumpNominalCurrent(); - -float GetInternalBatteryVoltage(); +float GetNernstAc(int ch); +float GetSensorInternalResistance(int ch); +float GetSensorTemperature(int ch); +float GetNernstDc(int ch); +float GetPumpNominalCurrent(int ch); +float GetInternalBatteryVoltage(int ch); diff --git a/firmware/uart.cpp b/firmware/uart.cpp index 532d2aa..6e8ee51 100644 --- a/firmware/uart.cpp +++ b/firmware/uart.cpp @@ -33,33 +33,41 @@ static void UartThread(void*) while(true) { - float lambda = GetLambda(); - int lambdaIntPart = lambda; - int lambdaThousandths = (lambda - lambdaIntPart) * 1000; - int batteryVoltageMv = GetInternalBatteryVoltage() * 1000; - int duty = GetHeaterDuty() * 100; + int ch; - 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), - batteryVoltageMv, - describeHeaterState(GetHeaterState()), duty, - describeFault(GetCurrentFault())); - chnWrite(&SD1, (const uint8_t *)printBuffer, writeCount); - chThdSleepMilliseconds(50); + for (ch = 0; ch < AFR_CHANNELS; ch++) { + float lambda = GetLambda(ch); + int lambdaIntPart = lambda; + int lambdaThousandths = (lambda - lambdaIntPart) * 1000; + int batteryVoltageMv = GetInternalBatteryVoltage(ch) * 1000; + int duty = GetHeaterDuty(ch) * 100; + + size_t writeCount = chsnprintf(printBuffer, 200, + "[AFR%d]: %d.%03d DC %4d mV AC %4d mV Rint: %5d T: %4d C Ipump: %6d uA Vheater: %5d heater: %s (%d)\tfault: %s\r\n", + ch, + lambdaIntPart, lambdaThousandths, + (int)(GetNernstDc(ch) * 1000.0), + (int)(GetNernstAc(ch) * 1000.0), + (int)GetSensorInternalResistance(ch), + (int)GetSensorTemperature(ch), + (int)(GetPumpNominalCurrent(ch) * 1000), + batteryVoltageMv, + describeHeaterState(GetHeaterState(ch)), duty, + describeFault(GetCurrentFault(ch))); + chnWrite(&SD1, (const uint8_t *)printBuffer, writeCount); + } #if HAL_USE_SPI - writeCount = chsnprintf(printBuffer, 200, - "EGT: %d C (int %d C)\r\n", - (int)getEgtDrivers()[0].temperature, - (int)getEgtDrivers()[0].cold_joint_temperature); - chnWrite(&SD1, (const uint8_t *)printBuffer, writeCount); - chThdSleepMilliseconds(50); + for (ch = 0; ch < EGT_CHANNELS; ch++) { + size_t writeCount = chsnprintf(printBuffer, 200, + "EGT[%d]: %d C (int %d C)\r\n", + (int)getEgtDrivers()[ch].temperature, + (int)getEgtDrivers()[ch].cold_joint_temperature); + chnWrite(&SD1, (const uint8_t *)printBuffer, writeCount); + } #endif /* HAL_USE_SPI */ + + chThdSleepMilliseconds(100); } }