From 31da75f7f24c88a8b3caf6f6ead8953839d56db2 Mon Sep 17 00:00:00 2001
From: Ollie Walsh <ollie.walsh@gmail.com>
Date: Fri, 14 Apr 2023 13:54:03 +0100
Subject: [PATCH] Improve ADC accuracy

Use the charge current ADC when undocked to determine ADC offset,
tweak the resistors values, and disable SMPS powersaving during ADC.
---
 Firmware/LowLevel/src/main.cpp | 32 ++++++++++++++++++++++++++------
 Firmware/LowLevel/src/pins.h   |  6 ++++++
 2 files changed, 32 insertions(+), 6 deletions(-)

diff --git a/Firmware/LowLevel/src/main.cpp b/Firmware/LowLevel/src/main.cpp
index 806d814f..5200b584 100644
--- a/Firmware/LowLevel/src/main.cpp
+++ b/Firmware/LowLevel/src/main.cpp
@@ -56,11 +56,15 @@ SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250);
 /**
  * @brief Some hardware parameters
  */
-#define VIN_R1 10000.0f
-#define VIN_R2 1000.0f
+#define VIN_R1 9980.0f
+#define VIN_R2 998.0f
 #define R_SHUNT 0.003f
 #define CURRENT_SENSE_GAIN 100.0f
 
+int next_adc_offset_sample = 0;
+float adc_offset_samples[20] = {0};
+float adc_offset = 0.0f;
+
 #define BATT_ABS_MAX 28.7f
 #define BATT_ABS_Min 21.7f
 
@@ -601,16 +605,32 @@ void loop() {
     if (now - last_status_update_millis > STATUS_CYCLETIME) {
         updateNeopixel();
 
+        // Disable power saving during ADC
+        digitalWrite(PIN_SMPS_POWERSAVE, HIGH);
         status_message.v_battery =
-                (float) analogRead(PIN_ANALOG_BATTERY_VOLTAGE) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2);
-        status_message.v_charge =
-                (float) analogRead(PIN_ANALOG_CHARGE_VOLTAGE) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2);
+            ((float)analogRead(PIN_ANALOG_BATTERY_VOLTAGE) - adc_offset) * (3.33f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2);
 #ifndef IGNORE_CHARGING_CURRENT
         status_message.charging_current =
-                (float) analogRead(PIN_ANALOG_CHARGE_CURRENT) * (3.3f / 4096.0f) / (CURRENT_SENSE_GAIN * R_SHUNT);
+            ((float)analogRead(PIN_ANALOG_CHARGE_CURRENT) - adc_offset) * (3.33f / 4096.0f) / (CURRENT_SENSE_GAIN * R_SHUNT);
 #else
         status_message.charging_current = -1.0f;
 #endif
+        status_message.v_charge = ((float)analogRead(PIN_ANALOG_CHARGE_VOLTAGE) - adc_offset) * (3.33f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2);
+
+
+        // If undocked use charge current ADC to determine adc offset
+        if( status_message.v_charge < 3.0f ) {
+            adc_offset_samples[next_adc_offset_sample++] = (float)analogRead(PIN_ANALOG_CHARGE_CURRENT);
+            next_adc_offset_sample %= 20;
+
+            float tmp = 0.0f;
+            for(int i=0; i<20; i++) {
+                tmp += adc_offset_samples[i];
+            }
+            adc_offset = tmp / 20.0f;
+        }
+        digitalWrite(PIN_SMPS_POWERSAVE, LOW);
+
         status_message.status_bitmask = (status_message.status_bitmask & 0b11111011) | ((charging_allowed & 0b1) << 2);
         status_message.status_bitmask = (status_message.status_bitmask & 0b11011111) | ((sound_available & 0b1) << 5);
 
diff --git a/Firmware/LowLevel/src/pins.h b/Firmware/LowLevel/src/pins.h
index dd79ce9e..65e2cf70 100644
--- a/Firmware/LowLevel/src/pins.h
+++ b/Firmware/LowLevel/src/pins.h
@@ -1,5 +1,11 @@
 #pragma once
 
+#ifdef ARDUINO_RASPBERRY_PI_PICO_W
+#define PIN_SMPS_POWERSAVE 33
+#else
+#define PIN_SMPS_POWERSAVE 23
+#endif
+
 #ifdef HW_0_9_X
 #define PIN_IMU_CS 17
 #define PIN_ANALOG_BATTERY_VOLTAGE 27