From fd507cb6fbb85d05bbffaa1ac501f6873aa612b5 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Wed, 10 May 2017 00:29:52 +0800
Subject: [PATCH] average emission current samples, implement emission_ready

---
 firmware/src/loop_cathode.rs | 26 ++++++++++++++++++++++----
 1 file changed, 22 insertions(+), 4 deletions(-)

diff --git a/firmware/src/loop_cathode.rs b/firmware/src/loop_cathode.rs
index f24c9aa..4ac083d 100644
--- a/firmware/src/loop_cathode.rs
+++ b/firmware/src/loop_cathode.rs
@@ -16,6 +16,8 @@ const PID_PARAMETERS: pid::Parameters = pid::Parameters {
 pub struct Controller {
     fbi_target: f32,
     fbi_range: board::EmissionRange,
+    fbi_buffer: [f32; 16],
+    fbi_buffer_count: usize,
     last_fbi: Option<f32>,
     pid: pid::Controller,
 
@@ -31,6 +33,8 @@ impl Controller {
         Controller {
             fbi_target: 0.0,
             fbi_range: board::EmissionRange::Low,
+            fbi_buffer: [0.0; 16],
+            fbi_buffer_count: 0,
             last_fbi: None,
             pid: pid::Controller::new(PID_PARAMETERS),
 
@@ -44,17 +48,28 @@ impl Controller {
     pub fn adc_input(&mut self, fbi_sample: u16, fd_sample: u16, fv_sample: u16, fbv_sample: u16) {
         let fbi_voltage = ((fbi_sample as f32) - board::FBI_ADC_OFFSET)/board::FBI_ADC_GAIN;
         let fbi_r225 = fbi_voltage/board::FBI_R225;
-        self.last_fbi = Some(match self.fbi_range {
+        let fbi = match self.fbi_range {
             board::EmissionRange::Low => fbi_r225,
             board::EmissionRange::Med => {
                 let fd_voltage = ((fd_sample as f32) - board::FD_ADC_OFFSET)/board::FD_ADC_GAIN;
                 fbi_r225 + (fbi_voltage - fd_voltage)/board::FBI_R223
             },
             board::EmissionRange::High => {
-                let fd_voltage = 0.9;
+                let fd_voltage = 0.8;
                 fbi_r225 + (fbi_voltage - fd_voltage)/board::FBI_R224
             }
-        });
+        };
+        self.fbi_buffer[self.fbi_buffer_count] = fbi;
+        self.fbi_buffer_count += 1;
+        if self.fbi_buffer_count == self.fbi_buffer.len() {
+            let mut fbi_avg: f32 = 0.0;
+            for fbi in self.fbi_buffer.iter() {
+                fbi_avg += *fbi;
+            }
+            self.last_fbi = Some(fbi_avg/(self.fbi_buffer.len() as f32));
+            self.fbi_buffer_count = 0;
+        }
+
         self.last_fv = Some(fv_sample as f32/board::FV_ADC_GAIN);
         self.last_fbv = Some(fbv_sample as f32/board::FBV_ADC_GAIN);
     }
@@ -77,7 +92,10 @@ impl Controller {
     }
 
     pub fn emission_ready(&self) -> bool {
-        false
+        match self.last_fbi {
+            None => false,
+            Some(last_fbi) => (self.fbi_target - last_fbi).abs()/self.fbi_target < 0.02
+        }
     }
 
     pub fn bias_ready(&self) -> bool {