volt meter averaging

src/voltage.rs

@@ -14,10 +14,27 @@ const FACTOR: f64 = 3.3 * 2.0 / 4095.0;
 type Batt = Pin<Gpio26, FunctionNull, PullNone>;
 type Usb = Pin<Gpio28, FunctionNull, PullNone>;
 
+trait Abs {
+    fn abs(self) -> Self;
+}
+
+impl Abs for f64 {
+    fn abs(self) -> Self {
+        if self > 0.0 {
+            self
+        } else {
+            -self
+        }
+    }
+}
+
 pub struct VoltMeter {
     batt: AdcPin<Batt>,
     usb: AdcPin<Usb>,
     adc: Adc,
+
+    batt_average: f64,
+    usb_average: f64,
 }
 
 impl VoltMeter {
@@ -25,10 +42,44 @@ impl VoltMeter {
         let batt = AdcPin::new(batt);
         let usb = AdcPin::new(usb);
 
-        Self { batt, usb, adc }
+        let mut s = Self {
+            batt,
+            usb,
+            adc,
+
+            batt_average: 0.0,
+            usb_average: 0.0,
+        };
+
+        let (cbat, cusb) = s.cur_voltage();
+        s.batt_average = cbat;
+        s.usb_average = cusb;
+
+        s
     }
 
     pub fn voltage(&mut self) -> (f64, f64) {
+        let (cbat, cusb) = self.cur_voltage();
+
+        if (self.batt_average - cbat).abs() > 0.1 {
+            self.batt_average = cbat;
+        } else {
+            // geometric averaging
+            self.batt_average *= 0.95;
+            self.batt_average += cbat * 0.05;
+        }
+
+        if (self.usb_average - cusb).abs() > 0.1 {
+            self.usb_average = cusb;
+        } else {
+            self.usb_average *= 0.95;
+            self.usb_average += cusb * 0.05;
+        }
+
+        (self.batt_average, self.usb_average)
+    }
+
+    fn cur_voltage(&mut self) -> (f64, f64) {
         let batt: u16 = self.adc.read(&mut self.batt).unwrap();
         let usb: u16 = self.adc.read(&mut self.usb).unwrap();