mirror of https://github.com/rusefi/wideband.git
107 lines
3.0 KiB
C++
107 lines
3.0 KiB
C++
#include "sampling.h"
|
|
|
|
#include "ch.h"
|
|
#include "hal.h"
|
|
|
|
#include "wideband_config.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;
|
|
|
|
constexpr float f_abs(float x)
|
|
{
|
|
return x > 0 ? x : -x;
|
|
}
|
|
|
|
static THD_WORKING_AREA(waSamplingThread, 256);
|
|
|
|
static void SamplingThread(void*)
|
|
{
|
|
float r_2 = 0;
|
|
float r_3 = 0;
|
|
|
|
while(true)
|
|
{
|
|
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.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 - result.VirtualGroundVoltageInt);
|
|
|
|
#ifdef BATTERY_INPUT_DIVIDER
|
|
internalBatteryVoltage = result.BatteryVoltage;
|
|
#endif
|
|
|
|
// Shift history over by one
|
|
r_3 = r_2;
|
|
r_2 = r_1;
|
|
}
|
|
}
|
|
|
|
void StartSampling()
|
|
{
|
|
adcStart(&ADCD1, nullptr);
|
|
chThdCreateStatic(waSamplingThread, sizeof(waSamplingThread), NORMALPRIO + 5, SamplingThread, nullptr);
|
|
}
|
|
|
|
float GetNernstAc()
|
|
{
|
|
return nernstAc;
|
|
}
|
|
|
|
float GetSensorInternalResistance()
|
|
{
|
|
// 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);
|
|
|
|
// 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 GetNernstDc()
|
|
{
|
|
return nernstDc;
|
|
}
|
|
|
|
float GetPumpNominalCurrent()
|
|
{
|
|
// 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;
|
|
}
|
|
|
|
float GetInternalBatteryVoltage()
|
|
{
|
|
return internalBatteryVoltage;
|
|
}
|