thermostat: Fix incorrect Dac Calibration Algo

- Thermostat Firmware repo PR #133

src/thermostat/thermostat.rs

@@ -45,6 +45,12 @@ pub struct TecSettings {
     pub vref: ElectricPotential,
 }
 
+#[derive(Deserialize, Serialize, Clone, Copy, Debug, Tree, Default)]
+pub struct Time {
+    ms: u32,
+    us: u32,
+}
+
 impl TecSettings {
     pub const TEC_VSEC_BIAS_V: ElectricPotential = ElectricPotential {
         dimension: PhantomData,
@@ -111,6 +117,8 @@ impl Default for TecSettings {
 pub struct Thermostat {
     max1968: MAX1968,
     ad7172: ad7172::AdcPhy,
+    interval: Time,
+    prev_ts: Time,
     pub tec_settings: TecSettings,
     pid_ctrl_ch0: PidState,
     temp_mon: TempMon,
@@ -133,6 +141,8 @@ impl Thermostat {
         Thermostat {
             max1968: max1968,
             ad7172: ad7172,
+            interval: Time::default(),
+            prev_ts: Time::default(),
             tec_settings: TecSettings::default(),
             pid_ctrl_ch0: PidState::default(),
             temp_mon: TempMon::default(),
@@ -186,10 +196,31 @@ impl Thermostat {
             debug!("state.get_pid_engaged(): {:?}", pid_engaged);
             debug!("Temperature: {:?} degree", temp);
             data_rdy = true;
+
+            let (ms, us) = sys_timer::now_precise();
+            if us < self.prev_ts.us {
+                self.interval = Time { 
+                    ms: ms - self.prev_ts.ms - 1,
+                    us: (us + 1000).abs_diff(self.prev_ts.us),
+                };
+            } else {
+                self.interval = Time { 
+                    ms: ms - self.prev_ts.ms,
+                    us: (us).abs_diff(self.prev_ts.us),
+                };
+            }
+            self.prev_ts = Time {
+                ms: ms,
+                us: us,
+            };
         });
         data_rdy
     }
 
+    pub fn get_poll_interval(&mut self) -> Time {
+        self.interval
+    }
+
     pub fn update_pid(&mut self) {
         let state: &mut PidState = &mut self.pid_ctrl_ch0;
         let pid_engaged = state.get_pid_engaged();
@@ -321,7 +352,6 @@ impl Thermostat {
         let mut start_value = 1;
         let mut best_error = ElectricPotential::new::<volt>(100.0);
         for step in (DAC_BIT - ADC_BIT - 1..DAC_BIT).rev() {
-            let mut prev_value = start_value;
             for value in (start_value..=ad5680::MAX_VALUE).step_by(1 << step) {
                 self.max1968.phy.dac.set(value).unwrap();
                 sys_timer::sleep(5);
@@ -332,12 +362,11 @@ impl Thermostat {
                     break;
                 } else if error < best_error {
                     best_error = error;
-                    start_value = prev_value;
+                    start_value = value;
 
                     let vref = (value as f32 / ad5680::MAX_VALUE as f32) * self.max1968.dac_out_range;
                     self.set_center_pt(vref);
                 }
-                prev_value = value;
             }
         }
         self.tec_settings.vref = target_voltage;
@@ -363,6 +392,7 @@ impl Thermostat {
         }
 
         StatusReport {
+            interval: self.get_poll_interval(),
             pwr_on: self.max1968.is_powered_on(),
             pid_engaged: self.get_pid_engaged(),
             temp_mon_status: self.temp_mon.get_status(),
@@ -563,6 +593,7 @@ impl Thermostat {
 
 #[derive(Deserialize, Serialize, Copy, Clone, Debug, Tree)]
 pub struct StatusReport {
+    interval: Time,
     pwr_on: bool,
     pid_engaged: bool,
     temp_mon_status: TempStatus,