finish and document CAN message types, add engine, ereg subsystems to FC

Author: tomcru4

firmwares/flight_computer/Cargo.toml

@@ -70,5 +70,8 @@ telemetry = { path = "../../telemetry" }
 defmt-test = "0.3"
 
 [features]
-gcs = []
-rev1 = []
+default = ["ethernet"]
+# gcs = []
+# rev1 = []
+usb = []
+ethernet = []

firmwares/flight_computer/src/bin/rocket.rs

@@ -96,6 +96,7 @@ async fn main(low_priority_spawner: Spawner) -> ! {
     #[cfg(feature = "usb")]
     let (usb_downlink, usb_uplink) = fw::usb::start(board.usb_driver, low_priority_spawner);
 
+    #[cfg(feature = "ethernet")]
     let (eth_downlink, eth_uplink) = fw::ethernet::start(board.ethernet, low_priority_spawner, seed);
 
     // subsystems
@@ -120,6 +121,7 @@ async fn main(low_priority_spawner: Spawner) -> ! {
         (can2_tx_pub, can2_rx_sub),
         #[cfg(feature = "usb")]
         (usb_downlink, usb_uplink),
+        #[cfg(feature = "ethernet")]
         (eth_downlink, eth_uplink),
         (lora_downlink, lora_uplink),
         subsystems,

firmwares/flight_computer/src/buzzer.rs

@@ -1,6 +1,7 @@
 //! Driver for the on-board buzzer, responsible for playing mode change beeps and
 //! warning tones using the STM32's timers for PWM generation.
 
+use defmt::warn;
 use embassy_executor::Spawner;
 use embassy_futures::select::{Either, select};
 use embassy_stm32::pac::gpio::Gpio;
@@ -305,7 +306,6 @@ const NO_BATTERY_ATTACHED_MELODY: [Note; 4] = [
 pub async fn run(mut pwm: SimplePwm<'static, embassy_stm32::peripherals::TIM2>, channel: Channel) -> ! {
     let max_duty = pwm.max_duty_cycle();
 
-    pwm.channel(channel).enable();
     pwm.channel(channel).set_duty_cycle(max_duty / 2);
 
     let mut flight_mode = FlightMode::default();

firmwares/flight_computer/src/can.rs

@@ -1,31 +1,29 @@
 use defmt::*;
 use embassy_executor::SendSpawner;
-// use embassy_stm32::can::bxcan::{Frame, Id, StandardId};
 use embassy_stm32::can::{Can, CanRx, CanTx, Frame};
-// use embassy_stm32::peripherals::CAN;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
-
 use embassy_sync::pubsub::{PubSubChannel, Publisher, Subscriber};
+
 use embedded_can::{Id, StandardId};
 use heapless::Vec;
-use nalgebra::inf;
 use static_cell::StaticCell;
 
 use shared_types::can::{
     CanMessage,
     structure::{Can2aFrame, CanFrameId},
 };
 
-pub const CAN_QUEUE_SIZE: usize = 20;
+pub const CAN_RX_QUEUE_SIZE: usize = 20;
+pub const CAN_TX_QUEUE_SIZE: usize = 20;
 pub const NUM_CAN_SUBSCRIBERS: usize = 1;
 pub const NUM_CAN_PUBLISHERS: usize = 1;
 
-pub type CanRxChannel = PubSubChannel<CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
+pub type CanRxChannel = PubSubChannel<CriticalSectionRawMutex, CanMessage, CAN_RX_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
 pub type CanRxSubscriber =
-    Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
-pub type CanTxChannel = PubSubChannel<CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
+    Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_RX_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
+pub type CanTxChannel = PubSubChannel<CriticalSectionRawMutex, CanMessage, CAN_TX_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
 pub type CanTxPublisher =
-    Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
+    Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_TX_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
 
 // --- can1
 pub static CAN1_RX_CH: StaticCell<CanRxChannel> = StaticCell::new();
@@ -41,12 +39,11 @@ pub static CAN2_TX_CH: StaticCell<CanTxChannel> = StaticCell::new();
 static CAN2_TX: StaticCell<CanTx<'static>> = StaticCell::new();
 static CAN2_RX: StaticCell<CanRx<'static>> = StaticCell::new();
 
+// --- dedicated tasks for receiving and sending CAN messages for each hardware Bus
 async fn run_can_rx(can_rx: &'static mut CanRx<'static>, publisher: CanTxPublisher) -> ! {
-    info!("Run Can RX");
     loop {
         match can_rx.read().await {
             Ok(envelope) => {
-                info!("Read Can Message");
                 let frame = envelope.frame;
                 let Id::Standard(id) = frame.id() else {
                     warn!("Unexpected extended 29bit Can Frame Id, dropping.");
@@ -58,52 +55,46 @@ async fn run_can_rx(can_rx: &'static mut CanRx<'static>, publisher: CanTxPublish
                 };
                 let Ok(id) = CanFrameId::try_from(id.as_raw()) else {
                     warn!(
-                        "Can Frame could not be converted, this is a bug in the implementation and should never happen"
+                        "Can Frame could not parsed into type, either the senders or this software might be outdated."
                     );
                     continue;
                 };
                 let message = Can2aFrame { id, payload: data };
                 let Ok(message_parsed) = CanMessage::try_from(message) else {
-                    warn!("Malformed Can Message, dropping"); //id: {}, payload: {}", message.id, message.payload);
+                    warn!("Malformed Can Message, message could not be parsed into type, dropping"); //id: {}, payload: {}", message.id, message.payload);
                     continue;
                 };
                 publisher.publish_immediate(message_parsed); // TODO: think about publish immediate
             }
             Err(e) => {
-                error!("Failed to read can envelope: {:?}", Debug2Format(&e))
+                error!("Can Bus Error: Failed to read can envelope: {:?}", Debug2Format(&e))
             }
         }
     }
 }
 
-async fn run_can_tx(
-    can_tx: &'static mut CanTx<'static>,
-    // mut subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
-    mut subscriber: CanRxSubscriber,
-) -> ! {
-    info!("Run can tx");
+async fn run_can_tx(can_tx: &'static mut CanTx<'static>, mut subscriber: CanRxSubscriber) -> ! {
     loop {
         let message = subscriber.next_message_pure().await; // should we care about lag?
-        info!("Can message transmit");
         let frame = Can2aFrame::from(message);
 
         let Some(sid) = StandardId::new(frame.id.into()) else {
+            error!("Can2.0A Frame could not be converted to StandardId, this is a bug in the implementation.");
             continue;
         };
 
         // unwrap is safe, because payload slice is never larger than 8 bytes
         let frame = Frame::new_data(sid, frame.payload.as_slice()).unwrap();
-        info!("embassy can frame write start");
         can_tx.write(&frame).await;
-        info!("embassy can frame write finished");
     }
 }
 
+// --- CAN1
 pub async fn spawn_can1(
     can: Can<'static>,
     spawner: SendSpawner,
-    publisher: Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
-    subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
+    publisher: Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_RX_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
+    subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_RX_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
 ) {
     let (can_tx, can_rx, _properties) = can.split();
     let can_tx = CAN1_TX.init(can_tx);
@@ -114,11 +105,7 @@ pub async fn spawn_can1(
 }
 
 #[embassy_executor::task]
-async fn run_can1_tx(
-    can_tx: &'static mut CanTx<'static>,
-    // mut subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
-    mut subscriber: CanRxSubscriber,
-) -> ! {
+async fn run_can1_tx(can_tx: &'static mut CanTx<'static>, mut subscriber: CanRxSubscriber) -> ! {
     run_can_tx(can_tx, subscriber).await
 }
 
@@ -131,8 +118,8 @@ async fn run_can1_rx(can_rx: &'static mut CanRx<'static>, publisher: CanTxPublis
 pub async fn spawn_can2(
     can: Can<'static>,
     spawner: SendSpawner,
-    publisher: Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
-    subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
+    publisher: Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_RX_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
+    subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_RX_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
 ) {
     let (can_tx, can_rx, _properties) = can.split();
     let can_tx = CAN2_TX.init(can_tx);

firmwares/flight_computer/src/lib.rs

@@ -297,11 +297,11 @@ pub async fn init_board()
     let lora2_iv = GenericSx126xInterfaceVariant::new(lora2_reset, lora2_irq, lora2_busy, None, None).unwrap();
     let mut lora2 = LoRa::new(Sx126x::new(lora2_spi, lora2_iv, lora2_config), false, Delay).await.unwrap();
 
-    let mut can1 = CanConfigurator::new(p.FDCAN1, p.PB8, p.PB9, Irqs);
-    let mut can2 = CanConfigurator::new(p.FDCAN2, p.PB5, p.PB6, Irqs);
+    let mut can1 = CanConfigurator::new(p.FDCAN2, p.PB5, p.PB6, Irqs);
+    let mut can2 = CanConfigurator::new(p.FDCAN1, p.PB8, p.PB9, Irqs);
 
-    can1.set_bitrate(1_000_000);
-    can2.set_bitrate(1_000_000);
+    can1.set_bitrate(125_000);
+    can2.set_bitrate(125_000);
 
     let mut can1 = can1.into_normal_mode();
     let mut can2 = can2.into_normal_mode();

firmwares/flight_computer/src/lora.rs

@@ -283,11 +283,13 @@ async fn run_gcs_downlink(
             let timeout = Duration::from_millis(DOWNLINK_PACKET_INTERVAL_MILLIS - 10);
             if let Some((msg, status)) = attempt_receive_downlink(&mut lora, &key, f, timeout, &local_settings).await {
                 downlink_sender.send(msg.clone()).await;
-                downlink_sender.send(DownlinkMessage::TelemetryGCS(shared_types::telemetry::TelemetryGCS {
-                    time: msg.time_produced().unwrap_or(0),
-                    lora_rssi: status.rssi as i32,
-                    lora_snr: status.snr as i32,
-                })).await;
+                downlink_sender
+                    .send(DownlinkMessage::TelemetryGCS(shared_types::telemetry::TelemetryGCS {
+                        time: msg.time_produced().unwrap_or(0),
+                        lora_rssi: status.rssi as i32,
+                        lora_snr: status.snr as i32,
+                    }))
+                    .await;
                 failure_counter = 0;
             } else {
                 failure_counter += 1;
@@ -325,7 +327,9 @@ async fn attempt_receive_downlink(
     lora.prepare_for_rx(RxMode::Continuous, &mod_params, &packet_params).await.unwrap();
 
     match with_timeout(timeout, lora.rx(&packet_params, &mut rx_buffer)).await {
-        Ok(Ok((len, status))) => decode_and_verify::<_, DOWNLINK_HMAC_LEN>(&key, &mut rx_buffer[..(len as usize)]).map(|msg| (msg, status)),
+        Ok(Ok((len, status))) => {
+            decode_and_verify::<_, DOWNLINK_HMAC_LEN>(&key, &mut rx_buffer[..(len as usize)]).map(|msg| (msg, status))
+        }
         _ => None,
     }
 }
@@ -347,8 +351,8 @@ async fn run_gcs_uplink(
 
         let serialized = serialize_and_hmac::<_, UPLINK_HMAC_LEN>(&key, &msg);
         tx_buffer[..serialized.len()].copy_from_slice(&serialized);
-        
-        for _ in 0..3 { 
+
+        for _ in 0..3 {
             let mod_params = lora.create_modulation_params(UPLINK_SF, UPLINK_BW, UPLINK_CR, frequency).unwrap();
             let mut packet_params =
                 lora.create_rx_packet_params(4, false, UPLINK_MSG_LEN as u8, true, false, &mod_params).unwrap();

firmwares/flight_computer/src/subsystems/engine.rs

@@ -0,0 +1,133 @@
+use defmt::info;
+use embassy_time::{Duration, Instant};
+use shared_types::can::{
+    CanMessage, SubsystemCommand, SubsystemInfo,
+    engine::{EngineInfoMsg, EngineState, LaunchCode},
+};
+
+use super::{ConnectivityStatus, Subsystem};
+use crate::can::CanTxPublisher;
+
+const CONECTIVITY_THRESHOLD: Duration = Duration::from_secs(2);
+const MIN_MSG_INTERVAL: Duration = Duration::from_millis(1000);
+
+#[derive(Default, Debug, Copy, Clone, PartialEq, Eq, Hash)]
+pub struct EngineMeasurements {
+    /// Pressure at the bottom of the oxidizer tank
+    pub pressure_ox: u8,
+    /// Pressure at combustion chamber
+    pub pressure_combustion_chamber: u16,
+    /// Temperature of the oxidizer / oxidizer tank
+    pub temp_ox: u8,
+    /// How open the main valve is: 1 = open to 0 = closed
+    pub main_valve: u8,
+    /// How open the main valve is: 1 = open to 0 = closed
+    pub fill_and_dump_valve: u8,
+}
+impl From<EngineInfoMsg> for EngineMeasurements {
+    fn from(value: EngineInfoMsg) -> Self {
+        EngineMeasurements {
+            pressure_ox: value.pressure_ox,
+            pressure_combustion_chamber: value.pressure_combustion_chamber,
+            temp_ox: value.temp_ox,
+            main_valve: value.main_valve,
+            fill_and_dump_valve: value.fill_and_dump_valve,
+        }
+    }
+}
+
+#[derive(Default)]
+pub struct EngineSubsystem {
+    target_state: EngineState,
+    // last state reported by the engine
+    current_state: Option<EngineState>,
+    // last measurements reported by the engine
+    measurements: Option<EngineMeasurements>,
+    // timestamp, for keeping track of connection quality
+    last_received_message: Option<(CanMessage, Instant)>,
+    // timestamp for preventing successive repeats
+    last_sent_state_message: Option<Instant>,
+    // Contains launch code
+    ignition: Option<u64>,
+}
+impl EngineSubsystem {
+    /// Send messages based on internal state, update internal state.
+    pub async fn tick(&mut self, can_publisher: &mut CanTxPublisher) {
+        self.check_publish_state(can_publisher).await;
+    }
+    /// Update internal state based on received CAN messages.
+    pub fn process_message(&mut self, message: CanMessage) {
+        if let CanMessage::Info(SubsystemInfo::Engine(msg)) = message {
+            self.last_received_message = Some((message, Instant::now()));
+            self.measurements = Some(msg.into());
+            self.current_state = Some(msg.engine_state);
+        }
+    }
+    pub fn set_target_state(&mut self, state: EngineState) {
+        if state == EngineState::Abort {
+            self.ignition = None;
+        }
+        if self.target_state != state {
+            // reset spam protection
+            self.last_sent_state_message = None;
+        }
+        // should we verify this here?
+        self.target_state = state;
+    }
+    pub fn set_ignition_true(&mut self, code: u64) {
+        self.target_state = EngineState::Ignition;
+        self.ignition = Some(code);
+    }
+    pub fn get_engine_measurements(&self) -> Option<EngineMeasurements> {
+        self.measurements
+    }
+
+    async fn check_publish_state(&mut self, can_publisher: &mut CanTxPublisher) {
+        let mut needs_publishing: bool = match self.target_state {
+            EngineState::Disarmed
+            | EngineState::Filling
+            | EngineState::Ready
+            | EngineState::Ignition
+            | EngineState::Abort
+            | EngineState::Scrub
+            | EngineState::Dump => true,
+            EngineState::Burn => false,
+        };
+        let state_automatically_changed = self.current_state == Some(EngineState::Burn);
+        let states_differ =
+            !state_automatically_changed && (self.target_state != self.current_state.unwrap_or_default());
+        // TODO: check anomalies with automatic burn state
+        let min_time_elapsed =
+            self.last_sent_state_message.map(|inst| inst.elapsed() > MIN_MSG_INTERVAL).unwrap_or(true);
+        let unresponsive = self.conectivity_status() != ConnectivityStatus::Online;
+
+        if needs_publishing && min_time_elapsed && (states_differ | unresponsive) {
+            let msg = match self.ignition {
+                // Don't send ignition in abort state.
+                Some(code) if self.target_state == EngineState::Ignition => {
+                    CanMessage::Cmd(SubsystemCommand::IgniteEngine(LaunchCode(code)))
+                }
+                Some(_) | None => CanMessage::Cmd(SubsystemCommand::SetEngineState(self.target_state)),
+            };
+            // NOTE: think about publish_immediate
+            can_publisher.publish_immediate(msg);
+            // info!("Sending 'SetEngineState' Can Message");
+            self.last_sent_state_message = Some(Instant::now());
+        }
+    }
+}
+
+impl Subsystem for EngineSubsystem {
+    fn conectivity_status(&self) -> super::ConnectivityStatus {
+        match self.last_received_message {
+            None => ConnectivityStatus::Offline,
+            Some((_, inst)) => {
+                if inst.elapsed() > CONECTIVITY_THRESHOLD {
+                    ConnectivityStatus::OfflineAgain
+                } else {
+                    ConnectivityStatus::Online
+                }
+            }
+        }
+    }
+}