From caec6f1a3b2c714dcc5afed1d7c3c6eafad3809e Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Tue, 9 May 2017 19:07:59 +0800
Subject: [PATCH] compute FV and FBV

---
 firmware/src/board.rs        |  3 +++
 firmware/src/loop_cathode.rs | 36 +++++++++++++++++++++++++++++++++---
 firmware/src/main.rs         |  7 ++++---
 3 files changed, 40 insertions(+), 6 deletions(-)

diff --git a/firmware/src/board.rs b/firmware/src/board.rs
index 3ef6f01..f9ea022 100644
--- a/firmware/src/board.rs
+++ b/firmware/src/board.rs
@@ -30,6 +30,9 @@ const UART_DIV_16P6: u32 = /*altclk*/16_000_000 * (1 << /*len(divfrac)*/6) /
 
 
 pub const AV_ADC_GAIN: f32 = 6.792703150912105;
+pub const FV_ADC_GAIN: f32 = 501.83449105726623;
+pub const FBV_ADC_GAIN: f32 = 49.13796058269066;
+pub const FBV_PWM_GAIN: f32 = 0.5730803571428571;
 
 
 pub fn set_led(nr: u8, state: bool) {
diff --git a/firmware/src/loop_cathode.rs b/firmware/src/loop_cathode.rs
index e552244..097e43e 100644
--- a/firmware/src/loop_cathode.rs
+++ b/firmware/src/loop_cathode.rs
@@ -1,3 +1,5 @@
+use core::num::Float;
+
 use board;
 use pid;
 
@@ -12,17 +14,45 @@ const PID_PARAMETERS: pid::Parameters = pid::Parameters {
 };
 
 pub struct Controller {
-    pid: pid::Controller
+    pid: pid::Controller,
+    fbv_target: f32,
+    last_fv: Option<f32>,
+    last_fbv: Option<f32>
 }
 
 
 impl Controller {
     pub const fn new() -> Controller {
         Controller {
-            pid: pid::Controller::new(PID_PARAMETERS)
+            pid: pid::Controller::new(PID_PARAMETERS),
+            fbv_target: 0.0,
+            last_fv: None,
+            last_fbv: None,
         }
     }
 
-    pub fn adc_input(&mut self, _fbi_sample: u16, _fd_sample: u16, _fv_sample: u16, _fbv_sample: u16) {
+    pub fn adc_input(&mut self, _fbi_sample: u16, _fd_sample: u16, fv_sample: u16, fbv_sample: u16) {
+        self.last_fbv = Some(fbv_sample as f32/board::FBV_ADC_GAIN);
+        self.last_fv = Some(fv_sample as f32/board::FV_ADC_GAIN);
+    }
+
+    pub fn set_bias_target(&mut self, volts: f32) {
+        self.fbv_target = volts;
+        board::set_fbv_pwm((volts/board::FBV_PWM_GAIN) as u16);
+    }
+
+    pub fn emission_ready(&self) -> bool {
+        false
+    }
+
+    pub fn bias_ready(&self) -> bool {
+        match self.last_fbv {
+            None => false,
+            Some(last_fbv) => (self.fbv_target - last_fbv).abs() < 1.0
+        }
+    }
+
+    pub fn ready(&self) -> bool {
+        self.emission_ready() & self.bias_ready()
     }
 }
diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index 37de701..9511f0e 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -69,9 +69,10 @@ fn main() {
         nvic.enable(Interrupt::ADC0SS0);
 
         board::set_emission_range(board::EmissionRange::High);
-        LOOP_ANODE.borrow(cs).borrow_mut().set_target(30.0+12.0);
-        //set_fv_pwm(10);
-        board::set_fbv_pwm(20);
+        let bias = 15.0;
+        LOOP_ANODE.borrow(cs).borrow_mut().set_target(70.0+bias);
+        LOOP_CATHODE.borrow(cs).borrow_mut().set_bias_target(bias);
+        //board::set_fv_pwm(10);
     });
 
     println!("ready");