From fbc6430aba8cb8ba8f2c1252b86aed9193247a9a Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Thu, 29 Oct 2020 02:55:55 -0700 Subject: [PATCH] implement sampling math --- firmware/Makefile | 5 ++- firmware/analog_input.cpp | 6 ++- firmware/main.cpp | 38 ++++++++++-------- firmware/sampling.cpp | 81 +++++++++++++++++++++++++++++++++++++++ firmware/sampling.h | 9 +++++ 5 files changed, 119 insertions(+), 20 deletions(-) create mode 100644 firmware/sampling.cpp create mode 100644 firmware/sampling.h diff --git a/firmware/Makefile b/firmware/Makefile index 86d286c..6f8675f 100644 --- a/firmware/Makefile +++ b/firmware/Makefile @@ -108,6 +108,7 @@ include $(CHIBIOS)/os/common/ports/ARMv6-M/compilers/GCC/mk/port.mk # Auto-build files in ./source recursively. include $(CHIBIOS)/tools/mk/autobuild.mk # Other files (optional). +include $(CHIBIOS)/os/hal/lib/streams/streams.mk # Define linker script file here LDSCRIPT= $(STARTUPLD)/STM32F042x6.ld @@ -125,6 +126,7 @@ CPPSRC = $(ALLCPPSRC) \ lambda_lookup.cpp \ pwm.cpp \ pump_dac.cpp \ + sampling.cpp \ main.cpp # List ASM source files here. @@ -151,7 +153,8 @@ CPPWARN = -Wall -Wextra -Wundef # # List all user C define here, like -D_DEBUG=1 -UDEFS = +UDEFS = -DCHPRINTF_USE_FLOAT=1 + # Define ASM defines here UADEFS = diff --git a/firmware/analog_input.cpp b/firmware/analog_input.cpp index ed8504d..c6ff389 100644 --- a/firmware/analog_input.cpp +++ b/firmware/analog_input.cpp @@ -3,7 +3,7 @@ #include "hal.h" #define ADC_CHANNEL_COUNT 3 -#define ADC_OVERSAMPLE 4 +#define ADC_OVERSAMPLE 16 static adcsample_t adcBuffer[ADC_CHANNEL_COUNT * ADC_OVERSAMPLE]; @@ -38,9 +38,11 @@ AnalogResult AnalogSample() { adcConvert(&ADCD1, &convGroup, adcBuffer, ADC_OVERSAMPLE); + constexpr float nernstInputGain = 1 / 2.7f; + return { - .NernstVoltage = AverageSamples(adcBuffer, 0), + .NernstVoltage = AverageSamples(adcBuffer, 0) * nernstInputGain, .VirtualGroundVoltage = AverageSamples(adcBuffer, 1), .PumpCurrentVoltage = AverageSamples(adcBuffer, 2), }; diff --git a/firmware/main.cpp b/firmware/main.cpp index 1d956ee..f4b9c72 100644 --- a/firmware/main.cpp +++ b/firmware/main.cpp @@ -1,10 +1,11 @@ #include "ch.h" #include "hal.h" +#include "chprintf.h" -#include "analog_input.h" #include "can.h" #include "pwm.h" #include "pump_dac.h" +#include "sampling.h" // 400khz / 1024 = 390hz PWM // TODO: this is wired to an inverted output, what do? @@ -20,12 +21,14 @@ static const UARTConfig uartCfg = .timeout_cb = nullptr, .timeout = 0, - .speed = 230400, + .speed = 500000, .cr1 = 0, .cr2 = 0, .cr3 = 0, }; +char strBuffer[200]; + /* * Application entry point. */ @@ -33,35 +36,36 @@ int main() { halInit(); chSysInit(); + StartSampling(); + InitPumpDac(); InitCan(); uartStart(&UARTD1, &uartCfg); - adcStart(&ADCD1, nullptr); - heaterPwm.Start(); heaterPwm.SetDuty(0.2f); - while (true) { -// auto result = AnalogSample(); -// // dummy data -// SendCanData(0.5f, 300); + /*for (int i = 0; i < 500; i++) { + SetPumpCurrentTarget(current); + chThdSleepMilliseconds(50); -// uartStartSend(&UARTD1, 13, "Hello, world!"); -// chThdSleepMilliseconds(10); + auto result = AnalogSample(); + //size_t writeCount = chsnprintf(strBuffer, 200, "I: %d\t\tVM: %.3f\tIp: %.3f\n", current, result.VirtualGroundVoltage, result.PumpCurrentVoltage); + size_t writeCount = chsnprintf(strBuffer, 200, "%d\t%.4f\n", current, result.PumpCurrentVoltage); + uartStartSend(&UARTD1, writeCount, strBuffer); - SetPumpCurrentTarget(-1000); - chThdSleepMilliseconds(10); - - SetPumpCurrentTarget(0); - chThdSleepMilliseconds(10); + //current += 10; + }*/ - SetPumpCurrentTarget(1000); - chThdSleepMilliseconds(10); + while(1) { + size_t writeCount = chsnprintf(strBuffer, 200, "%.4f\t%.2f\n", GetNernstDc() * 1000, GetSensorInternalResistance()); + uartStartSend(&UARTD1, writeCount, strBuffer); + + chThdSleepMilliseconds(5); } } diff --git a/firmware/sampling.cpp b/firmware/sampling.cpp new file mode 100644 index 0000000..86f97ce --- /dev/null +++ b/firmware/sampling.cpp @@ -0,0 +1,81 @@ +#include "sampling.h" + +#include "ch.h" +#include "hal.h" + +#include "analog_input.h" + +// Stored results +float nernstAc = 0; +float nernstDc = 0; +volatile float pumpCurrentSenseVoltage = 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) + { + // First toggle the pin + palTogglePad(GPIOB, 8); + auto result = AnalogSample(); + + float r_1 = result.NernstVoltage; + + // Compute results + + // 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 + float r2_opposite_phase = (r_1 + r_3) / 2; + + nernstAc = f_abs(r2_opposite_phase - r_2); + nernstDc = (r2_opposite_phase + r_2) / 2; + + pumpCurrentSenseVoltage = 0.8f * pumpCurrentSenseVoltage + 0.2f * (result.PumpCurrentVoltage - 1.65f); + + // 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 + return 22000 / (3.3f / GetNernstAc() - 1); +} + +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 / 619.0f; + return pumpCurrentSenseVoltage * ratio; +} diff --git a/firmware/sampling.h b/firmware/sampling.h new file mode 100644 index 0000000..037d9a4 --- /dev/null +++ b/firmware/sampling.h @@ -0,0 +1,9 @@ +#pragma once + +void StartSampling(); + +float GetNernstAc(); +float GetSensorInternalResistance(); +float GetNernstDc(); + +float GetPumpNominalCurrent();