Testing current actually works

src/main.rs

@@ -80,25 +80,17 @@ fn main() -> ! {
 
     let mut tick = 0u8;
 
-    let mut peak_v = 0u32;
-
     let mut current_pins = [
         pins.gpio16.into_push_pull_output().into_dyn_pin(),
         pins.gpio17.into_push_pull_output().into_dyn_pin(),
         pins.gpio18.into_push_pull_output().into_dyn_pin(),
     ];
 
+    let mut samps = [0u16; 8];
+
     loop {
-        delay.delay_ms(3000);
-        adc_fifo.clear();
-        adc_fifo.resume();
-
-        while adc_fifo.len() < 7 {
-            delay.delay_us(1);
-        }
-
-        adc_fifo.pause();
-
+        // set pwr lvl
+        tick = (tick + 1) % 2;
         for (i, cp) in current_pins.iter_mut().enumerate() {
             let mask = 0b1 << i;
 
@@ -111,7 +103,31 @@ fn main() -> ! {
 
         info!("PWR LVL: {}", tick);
 
-        tick = (tick + 1) % 8;
+        // stabilize
+        delay.delay_ms(3000);
+
+        // get avg current from shunt
+        adc_fifo.clear();
+        adc_fifo.resume();
+
+        while adc_fifo.len() < 7 {
+            delay.delay_us(1);
+        }
+
+        adc_fifo.pause();
+
+        for i in 0..8 {
+            samps[i] = adc_fifo.read();
+        }
+
+        let avg = avg_fifo(&samps, false);
+
+        let mv = (avg * 3300) / 4095; // (E-4)
+        let ua = mv * 10; // (E-8)
+
+        let power_mw = (mv * ua) as u64 / (1_000 * 1_000);
+
+        println!("{}mV, {}mA, {}mW\n", mv, mv / 100, power_mw);
     }
 }