Health manager update

.gitignore

@@ -1,4 +1,5 @@
 /target
 /.vscode/.cortex-debug.registers.state.json
 /.vscode/.cortex-debug.peripherals.state.json
-/.vscode/settings.json
+/.vscode/settings.json
+/.vs

boards/communication/src/health.rs

@@ -54,6 +54,12 @@ impl HealthMonitorChannels for HealthMonitorChannelsCommunication {
     fn get_failover(&mut self) -> Option<u16> {
         self.reader1.read(&mut self.pin_failover).ok()
     }
+    fn get_drogue_sense(&mut self) -> Option<u16> {
+        None
+    }
+    fn get_main_sense(&mut self) -> Option<u16> {
+        None
+    }
 }
 
 impl HealthMonitorChannelsCommunication {

boards/recovery/src/health.rs

@@ -0,0 +1,89 @@
+//! Health related code
+//! Would've liked to have this live in common-arm-atsame but the pins and adc are not standardised
+//! for all boards which poses the problem of giving an adc to a wrong pin in a generic way.
+//Pins
+use atsamd_hal::gpio::{
+    Alternate, Pin, B, PB00, PB01, PB02, PB03, PB05, PB08, PB09,
+};
+use atsamd_hal::{adc::Adc, ehal::adc::OneShot, pac::ADC0, pac::ADC1};
+
+//health monitor trait
+use common_arm::HealthMonitorChannels;
+
+// make sure to define the ADC types in types.rs
+
+// I don't think this should own the ADC object, but rather when a call to evaluate is invoke it should be taken control
+// and then released when the function returns. Refactor this later.
+pub struct HealthMonitorChannelsRecovery {
+    reader: Adc<ADC0>,
+    reader1: Adc<ADC1>,
+    pin_3v3: Pin<PB01, Alternate<B>>,
+    pin_5v: Pin<PB02, Alternate<B>>,
+    pin_pyro: Pin<PB03, Alternate<B>>,
+    pin_vcc: Pin<PB00, Alternate<B>>,
+    pin_failover: Pin<PB05, Alternate<B>>,
+    pin_drogue_sense: Pin<PB09, Alternate<B>>,
+    pin_main_sense: Pin<PB08, Alternate<B>>,
+}
+
+impl HealthMonitorChannels for HealthMonitorChannelsRecovery {
+    fn get_3v3(&mut self) -> Option<u16> {
+        self.reader.read(&mut self.pin_3v3).ok()
+    }
+    fn get_5v(&mut self) -> Option<u16> {
+        self.reader.read(&mut self.pin_5v).ok()
+    }
+    fn get_pyro(&mut self) -> Option<u16> {
+        self.reader.read(&mut self.pin_pyro).ok()
+    }
+    fn get_vcc(&mut self) -> Option<u16> {
+        self.reader.read(&mut self.pin_vcc).ok()
+    }
+    fn get_int_5v(&mut self) -> Option<u16> {
+        None
+    }
+    fn get_int_3v3(&mut self) -> Option<u16> {
+        None
+    }
+    fn get_ext_5v(&mut self) -> Option<u16> {
+        None
+    }
+    fn get_ext_3v3(&mut self) -> Option<u16> {
+        None
+    }
+    fn get_failover(&mut self) -> Option<u16> {
+        self.reader1.read(&mut self.pin_failover).ok()
+    }
+    fn get_drogue_sense(&mut self) -> Option<u16> {
+        self.reader1.read(&mut self.pin_drogue_sense).ok()
+    }
+    fn get_main_sense(&mut self) -> Option<u16> {
+        self.reader1.read(&mut self.pin_main_sense).ok()
+    }
+}
+
+impl HealthMonitorChannelsRecovery {
+    pub fn new(
+        reader: Adc<ADC0>,
+        reader1: Adc<ADC1>,
+        pin_3v3: Pin<PB01, Alternate<B>>,
+        pin_5v: Pin<PB02, Alternate<B>>,
+        pin_pyro: Pin<PB03, Alternate<B>>,
+        pin_vcc: Pin<PB00, Alternate<B>>,
+        pin_failover: Pin<PB05, Alternate<B>>,
+        pin_drogue_sense: Pin<PB09, Alternate<B>>,
+        pin_main_sense: Pin<PB08, Alternate<B>>,
+    ) -> Self {
+        HealthMonitorChannelsRecovery {
+            reader,
+            reader1,
+            pin_3v3,
+            pin_5v,
+            pin_pyro,
+            pin_vcc,
+            pin_failover,
+            pin_drogue_sense,
+            pin_main_sense,
+        }
+    }
+}

boards/recovery/src/main.rs

@@ -4,6 +4,7 @@
 mod communication;
 mod data_manager;
 mod gpio_manager;
+mod health;
 mod state_machine;
 mod types;
 
@@ -13,12 +14,17 @@ use atsamd_hal::clock::v2::Source;
 use atsamd_hal::dmac::DmaController;
 use common_arm::hinfo;
 use common_arm::mcan;
+use common_arm::HealthManager;
+use common_arm::HealthMonitor;
+use messages::health::Health;
 use common_arm::*;
 use communication::Capacities;
 use data_manager::DataManager;
 use gpio_manager::GPIOManager;
 use hal::gpio::{Pin, Pins, PushPullOutput, PB16, PB17};
 use hal::prelude::*;
+use hal::adc::Adc;
+use health::HealthMonitorChannelsRecovery;
 use mcan::messageram::SharedMemory;
 use messages::*;
 use panic_halt as _;
@@ -35,6 +41,7 @@ mod app {
         em: ErrorManager,
         data_manager: DataManager,
         can0: communication::CanDevice0,
+        health_manager: HealthManager<HealthMonitorChannelsRecovery>,
         gpio: GPIOManager,
     }
 
@@ -72,7 +79,7 @@ mod app {
 
         // SAFETY: Misusing the PAC API can break the system.
         // This is safe because we only steal the MCLK.
-        let (_, _, _, _mclk) = unsafe { clocks.pac.steal() };
+        let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() };
 
         /* CAN config */
         let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0);
@@ -87,6 +94,28 @@ mod app {
             false,
         );
 
+        /* Setup ADC clocks */
+        let (_pclk_adc0, gclk0) = Pclk::enable(tokens.pclks.adc0, gclk0);
+        let (_pclk_adc1, gclk0) = Pclk::enable(tokens.pclks.adc1, gclk0);
+        /* Setup ADC */
+        let adc0 = Adc::adc0(peripherals.ADC0, &mut mclk);
+        let adc1 = Adc::adc1(peripherals.ADC1, &mut mclk);
+
+        let health_monitor_channels = HealthMonitorChannelsRecovery::new(
+            adc0,
+            adc1,
+            pins.pb01.into(),
+            pins.pb02.into(),
+            pins.pb03.into(),
+            pins.pb00.into(),
+            pins.pb05.into(),
+            pins.pb09.into(),
+            pins.pb08.into(),
+        );
+
+        let health_monitor = HealthMonitor::new(health_monitor_channels, 10000, 5000, 1023);
+        let health_manager = HealthManager::new(health_monitor);
+
         /* GPIO config */
         let led_green = pins.pb16.into_push_pull_output();
         let led_red = pins.pb17.into_push_pull_output();
@@ -102,8 +131,7 @@ mod app {
         run_sm::spawn().ok();
         state_send::spawn().ok();
         blink::spawn().ok();
-        // fire_main::spawn_after(ExtU64::secs(15)).ok();
-        // fire_drogue::spawn_after(ExtU64::secs(15)).ok();
+        report_health::spawn().ok();
 
         /* Monotonic clock */
         let mono = Systick::new(core.SYST, gclk0.freq().to_Hz());
@@ -112,6 +140,7 @@ mod app {
             Shared {
                 em: ErrorManager::new(),
                 data_manager: DataManager::new(),
+                health_manager,
                 can0,
                 gpio,
             },
@@ -184,14 +213,12 @@ mod app {
     #[task(binds = CAN0, shared = [can0, data_manager, &em])]
     fn can0(mut cx: can0::Context) {
         cx.shared.can0.lock(|can| {
-            cx.shared
-                .data_manager
-                .lock(|data_manager| {
-                    cx.shared.em.run(|| {
-                        can.process_data(data_manager)?;
-                        Ok(())
-                    });
+            cx.shared.data_manager.lock(|data_manager| {
+                cx.shared.em.run(|| {
+                    can.process_data(data_manager)?;
+                    Ok(())
                 });
+            });
         });
     }
 
@@ -226,6 +253,26 @@ mod app {
         });
     }
 
+    /**
+     * Simple health report
+     */
+    #[task(shared = [&em, health_manager])]
+    fn report_health(mut cx: report_health::Context) {
+        cx.shared.em.run(|| {
+            let msg = cx.shared.health_manager.lock(|health_manager| {
+                let state = health_manager.evaluate();
+                Message::new(
+                    &monotonics::now(),
+                    COM_ID,
+                    Health::new(health_manager.monitor.data.clone(), state),
+                )
+            });
+            spawn!(send_internal, msg)?; // dump to the can bus
+            spawn_after!(report_health, ExtU64::secs(5))?;
+            Ok(())
+        });
+    }
+
     /// Simple blink task to test the system.
     /// Acts as a heartbeat for the system.
     #[task(local = [led_green, led_red], shared = [&em])]

libraries/common-arm/src/health/health_manager.rs

@@ -36,6 +36,8 @@ where
         let pyro_status = get_status(data.pyro_sense, &self.monitor.range_pyro);
         let vcc_status = get_status(data.vcc_sense, &self.monitor.range_vcc);
         let failover_status = get_status(data.failover_sense, &self.monitor.range_failover);
+        let drogue_sense = get_status(data.drogue_sense, &self.monitor.range_drogue_sense);
+        let main_sense = get_status(data.main_sense, &self.monitor.range_main_sense);
 
         for status in [
             v5_status,
@@ -47,6 +49,8 @@ where
             pyro_status,
             vcc_status,
             failover_status,
+            drogue_sense,
+            main_sense,
         ] {
             match status {
                 HealthState::Error => return HealthState::Error,
@@ -60,19 +64,19 @@ where
 }
 
 fn get_status(data: Option<u16>, nominal: &RangeInclusive<u16>) -> HealthState {
-    match data {
+    return match data {
         Some(x) => {
-            if nominal.contains(&x) {
-                return HealthState::Nominal;
-            }
-            else {
+            if !nominal.contains(&x) {
                 warn!("Unsafe Voltage");
                 HealthState::Error
             }
+            else {
+                HealthState::Nominal
+            }
         },
         None => {
             warn!("No data");
-            return HealthState::Warning;
+            HealthState::Warning
         }
     }
 }

libraries/common-arm/src/health/health_monitor.rs

@@ -17,6 +17,8 @@ pub trait HealthMonitorChannels {
     fn get_ext_5v(&mut self) -> Option<u16>;
     fn get_ext_3v3(&mut self) -> Option<u16>;
     fn get_failover(&mut self) -> Option<u16>;
+    fn get_drogue_sense(&mut self) -> Option<u16>;
+    fn get_main_sense(&mut self) -> Option<u16>;
 }
 
 pub struct HealthMonitor<T: HealthMonitorChannels> {
@@ -33,6 +35,8 @@ pub struct HealthMonitor<T: HealthMonitorChannels> {
     pub range_ext_5v: RangeInclusive<u16>,
     pub range_ext_3v3: RangeInclusive<u16>,
     pub range_failover: RangeInclusive<u16>,
+    pub range_drogue_sense: RangeInclusive<u16>,
+    pub range_main_sense: RangeInclusive<u16>,
 }
 
 impl<T> HealthMonitor<T>
@@ -62,6 +66,12 @@ where
         let range_failover = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2))
             as u16)
             ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16);
+        let range_drogue_sense = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9))
+            as u16)
+            ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16);
+        let range_main_sense = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9))
+            as u16)
+            ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16);
         Self {
             channels,
             data: HealthStatus {
@@ -74,6 +84,8 @@ where
                 ext_v5: None,
                 ext_3v3: None,
                 failover_sense: None,
+                drogue_sense: None,
+                main_sense: None,
             },
             range_5v,
             range_3v3,
@@ -84,6 +96,9 @@ where
             range_ext_5v,
             range_ext_3v3,
             range_failover,
+            range_drogue_sense,
+            range_main_sense,
+
         }
     }
 
@@ -102,6 +117,8 @@ where
         self.data.ext_v5 = self.channels.get_ext_5v();
         self.data.ext_3v3 = self.channels.get_ext_3v3();
         self.data.failover_sense = self.channels.get_failover();
+        self.data.drogue_sense = self.channels.get_drogue_sense();
+        self.data.main_sense = self.channels.get_drogue_sense();
     }
 }
 

libraries/messages/src/health.rs

@@ -56,6 +56,8 @@ pub struct HealthStatus {
     pub ext_v5: Option<u16>,
     pub ext_3v3: Option<u16>,
     pub failover_sense: Option<u16>,
+    pub drogue_sense: Option<u16>,
+    pub main_sense: Option<u16>,
 }
 
 impl Health {