hehehe cerberus in rust

Cargo.toml

@@ -25,6 +25,7 @@ heapless = { version = "0.8", default-features = false }
 static_cell = "2.1.0"
 chrono = { version = "^0.4", default-features = false}
 embedded-hal-async = {version = "1.0.0", features = ["defmt-03"] }
+bitfield = "^0.13.2"
 
 
 [patch.crates-io]

cerberus/Cargo.toml

@@ -17,4 +17,5 @@ embassy-futures.workspace = true
 heapless.workspace = true
 panic-probe.workspace = true
 static_cell.workspace = true
+bitfield.workspace = true
 pca9539-ner = { version = "0.1.0", path = "../crates/pca9539-ner" }

cerberus/src/can_handler.rs

@@ -35,7 +35,9 @@ pub async fn can_handler(
         match select(recv.receive(), can.read()).await {
             select::Either::First(frame) => {
                 trace!("Sending frame: {}", frame);
-                can.write(&frame).await;
+                if let Some(_) = can.write(&frame).await.dequeued_frame() {
+                    warn!("Dequeing can frames!");
+                }
             }
             select::Either::Second(res) => match res {
                 Ok(got) => match got.frame.header().id() {

cerberus/src/dti.rs

@@ -0,0 +1,30 @@
+use core::{f32::consts::PI, sync::atomic::AtomicI32};
+
+use embassy_stm32::can::Frame;
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
+
+#[embassy_executor::task]
+pub async fn dti_handler(
+    recved: &'static Signal<CriticalSectionRawMutex, Frame>,
+    speed: &'static AtomicI32,
+) {
+    loop {
+        let frame = recved.wait().await;
+        match frame.id() {
+            embassy_stm32::can::Id::Standard(id) => match id.as_raw() {
+                0x416 => {
+                    // TODO fat chance this works
+                    let erpm = ((frame.data()[0] as i32) << 24u32)
+                        + ((frame.data()[1] as i32) << 16)
+                        + ((frame.data()[2] as i32) << 8u32)
+                        + (frame.data()[3] as i32);
+                    let mph = (erpm / 10) as f32 / (47.0 / 13.0) * 60.0 * (16.0 / 63360.0) * PI;
+                    // TODO add precision
+                    speed.store(mph as i32, core::sync::atomic::Ordering::Release);
+                }
+                _ => (),
+            },
+            embassy_stm32::can::Id::Extended(_) => (),
+        }
+    }
+}

cerberus/src/fault.rs

@@ -6,34 +6,47 @@ use embassy_sync::{
     channel::Sender,
     signal::Signal,
 };
-use embassy_time::Timer;
+use embassy_time::{Duration, Instant, Timer};
 
-use crate::FaultCode;
+use crate::{FaultCode, FaultSeverity, FunctionalType, StateTransition};
 
 #[embassy_executor::task]
 pub async fn fault_handler(
     can_send: Sender<'static, ThreadModeRawMutex, Frame, 25>,
     fault: &'static Signal<CriticalSectionRawMutex, FaultCode>,
+    state_send: &'static Signal<CriticalSectionRawMutex, StateTransition>,
 ) {
     let mut last_fault = FaultCode::FaultsClear;
 
     let status_id: StandardId = unwrap!(StandardId::new(0x502));
 
     let mut fault_bits: [u8; 5] = [0u8; 5];
 
+    let mut last_fault_time = Instant::now();
+
     loop {
         last_fault = match select(fault.wait(), Timer::after_millis(250)).await {
             embassy_futures::select::Either::First(event) => {
                 match event.get_severity() {
                     crate::FaultSeverity::Defcon1
                     | crate::FaultSeverity::Defcon2
-                    | crate::FaultSeverity::Defcon3 => todo!("Fault"),
+                    | crate::FaultSeverity::Defcon3 => {
+                        state_send.signal(StateTransition::Functional(FunctionalType::FAULTED));
+                        last_fault_time = Instant::now();
+                    }
                     crate::FaultSeverity::Defcon4 => warn!("Non critical fault!"),
                     crate::FaultSeverity::Defcon5 => debug!("Faults clear!"),
                 }
                 event
             }
-            embassy_futures::select::Either::Second(_) => last_fault,
+            embassy_futures::select::Either::Second(_) => {
+                if last_fault.get_severity() as u8 <= FaultSeverity::Defcon3 as u8
+                    && Instant::now() - last_fault_time > Duration::from_secs(5)
+                {
+                    state_send.signal(StateTransition::Functional(FunctionalType::READY))
+                }
+                FaultCode::FaultsClear
+            }
         };
 
         fault_bits[3..4].copy_from_slice(&(last_fault.get_severity() as u8).to_be_bytes());

cerberus/src/lib.rs

@@ -2,14 +2,17 @@
 
 pub mod bms;
 pub mod can_handler;
+pub mod dti;
 pub mod fault;
+pub mod monitor;
+pub mod state_machine;
 
 pub type SharedI2c = embassy_sync::mutex::Mutex<
     embassy_sync::blocking_mutex::raw::NoopRawMutex,
     embassy_stm32::i2c::I2c<'static, embassy_stm32::mode::Async>,
 >;
 
-#[derive(Copy, Clone)]
+#[derive(Copy, Clone, PartialEq, Eq)]
 pub enum FunctionalType {
     READY,
     /* F means functional */
@@ -39,6 +42,7 @@ pub enum StateTransition {
 // TODO: this is a breaking change and is also ugly and non-exhuastive in terms of IDs
 // However it is centralized which is better than the C impl
 
+#[repr(u8)]
 pub enum FaultSeverity {
     Defcon1 = 1,
     Defcon2 = 2,
@@ -61,3 +65,10 @@ impl FaultCode {
         }
     }
 }
+
+pub enum PduCommand {
+    WritePump(bool),
+    WriteBrakelight(bool),
+    WriteFault(bool),
+    SoundRtds,
+}

cerberus/src/main.rs

@@ -1,9 +1,15 @@
 #![no_std]
 #![no_main]
 
-use core::fmt::Write;
+use core::{
+    fmt::Write,
+    sync::atomic::{AtomicBool, AtomicI32, AtomicU32},
+};
 
-use cerberus::{bms, can_handler, fault, FaultCode, SharedI2c, StateTransition};
+use cerberus::{
+    bms, can_handler, dti, fault, monitor, state_machine, FaultCode, PduCommand, SharedI2c,
+    StateTransition,
+};
 use cortex_m::{peripheral::SCB, singleton};
 use cortex_m_rt::{exception, ExceptionFrame};
 use defmt::{info, unwrap, warn};
@@ -12,6 +18,7 @@ use embassy_stm32::{
     adc::{Adc, SampleTime, Sequence},
     bind_interrupts,
     can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler},
+    exti::ExtiInput,
     i2c::{self, I2c},
     peripherals::CAN1,
     time::Hertz,
@@ -57,10 +64,18 @@ bind_interrupts!(struct IrqsI2c2 {
 });
 
 static CAN_CHANNEL: Channel<ThreadModeRawMutex, Frame, 25> = Channel::new();
+static PDU_COMMAND: Channel<ThreadModeRawMutex, PduCommand, 10> = Channel::new();
 
 static CURRENT_STATE: Signal<CriticalSectionRawMutex, StateTransition> = Signal::new();
 static FAULT: Signal<CriticalSectionRawMutex, FaultCode> = Signal::new();
 
+// true=TS ON
+static TSMS_SENSE: AtomicBool = AtomicBool::new(false);
+// true=brakes engaged
+static BRAKE_STATE: AtomicBool = AtomicBool::new(false);
+
+static DTI_MPH: AtomicI32 = AtomicI32::new(0);
+
 static BMS_CALLBACK: Signal<CriticalSectionRawMutex, Frame> = Signal::new();
 static DTI_CALLBACK: Signal<CriticalSectionRawMutex, Frame> = Signal::new();
 
@@ -83,8 +98,15 @@ async fn main(spawner: Spawner) -> ! {
     if let Err(err) = spawner.spawn(bms::bms_handler(&BMS_CALLBACK, &FAULT)) {
         warn!("Could not spawn BMS task: {}", err);
     }
+    if let Err(err) = spawner.spawn(dti::dti_handler(&DTI_CALLBACK, &DTI_MPH)) {
+        warn!("Could not spawn DTI task: {}", err);
+    }
 
-    if let Err(err) = spawner.spawn(fault::fault_handler(CAN_CHANNEL.sender(), &FAULT)) {
+    if let Err(err) = spawner.spawn(fault::fault_handler(
+        CAN_CHANNEL.sender(),
+        &FAULT,
+        &CURRENT_STATE,
+    )) {
         warn!("Could not spawn fault task: {}", err);
     }
 
@@ -113,24 +135,57 @@ async fn main(spawner: Spawner) -> ! {
         i2c::Config::default(),
     );
     let i2c_bus_2 = I2C_BUS_2.init(Mutex::new(i2c_2));
+    if let Err(err) = spawner.spawn(monitor::ctrl_expander_handler(
+        CAN_CHANNEL.sender(),
+        PDU_COMMAND.receiver(),
+        i2c_bus_2,
+        &TSMS_SENSE,
+    )) {
+        warn!("Could not spawn ctrl expander task: {}", err);
+    }
 
-    const ADC_BUF_SIZE: usize = 1024;
-
+    const ADC1_BUF_SIZE: usize = 40;
     let adc1 = Adc::new(p.ADC1);
-    let adc_data_1 = singleton!(ADCDAT : [u16; ADC_BUF_SIZE] = [0u16; ADC_BUF_SIZE])
+    let adc_data_1 = singleton!(ADCDAT : [u16; ADC1_BUF_SIZE] = [0u16; ADC1_BUF_SIZE])
         .expect("Could not init adc buffer");
     let mut adc1 = adc1.into_ring_buffered(p.DMA2_CH4, adc_data_1);
-    adc1.set_sample_sequence(Sequence::One, &mut p.PB0, SampleTime::CYCLES112); //
+    adc1.set_sample_sequence(Sequence::One, &mut p.PB0, SampleTime::CYCLES112); // LV sense
+    if let Err(err) = spawner.spawn(monitor::lv_sense_handler(adc1, CAN_CHANNEL.sender())) {
+        warn!("Could not spawn LV sense task: {}", err);
+    }
 
+    const ADC3_BUF_SIZE: usize = 120;
     let adc3 = Adc::new(p.ADC3);
-    let adc_data_3 = singleton!(ADCDAT : [u16; ADC_BUF_SIZE] = [0u16; ADC_BUF_SIZE])
+    let adc_data_3 = singleton!(ADCDAT : [u16; ADC3_BUF_SIZE] = [0u16; ADC3_BUF_SIZE])
         .expect("Could not init adc buffer");
     let mut adc3 = adc3.into_ring_buffered(p.DMA2_CH0, adc_data_3);
     adc3.set_sample_sequence(Sequence::One, &mut p.PA0, SampleTime::CYCLES112); //
     adc3.set_sample_sequence(Sequence::One, &mut p.PA1, SampleTime::CYCLES112); //
     adc3.set_sample_sequence(Sequence::One, &mut p.PA2, SampleTime::CYCLES112); //
     adc3.set_sample_sequence(Sequence::One, &mut p.PA3, SampleTime::CYCLES112); //
 
+    let button1 = ExtiInput::new(p.PA4, p.EXTI4, embassy_stm32::gpio::Pull::Up);
+    let button2 = ExtiInput::new(p.PA5, p.EXTI5, embassy_stm32::gpio::Pull::Up);
+    let button3 = ExtiInput::new(p.PA6, p.EXTI6, embassy_stm32::gpio::Pull::Up);
+    let button4 = ExtiInput::new(p.PA7, p.EXTI7, embassy_stm32::gpio::Pull::Up);
+    //let button5 = ExtiInput::new(p.PC4, p.EXTI4, embassy_stm32::gpio::Pull::Up);
+    //let button6 = ExtiInput::new(p.PC5, p.EXTI5, embassy_stm32::gpio::Pull::Up);
+    let button7 = ExtiInput::new(p.PB0, p.EXTI0, embassy_stm32::gpio::Pull::Up);
+    let button8 = ExtiInput::new(p.PB1, p.EXTI1, embassy_stm32::gpio::Pull::Up);
+    if let Err(err) = spawner.spawn(monitor::steeringio_handler(
+        CAN_CHANNEL.sender(),
+        button1,
+        button2,
+        button3,
+        button4,
+        // button5,
+        // button6,
+        button7,
+        button8,
+    )) {
+        warn!("Could not spawn steeringIO task: {}", err);
+    }
+
     let mut usart = Uart::new(
         p.USART3,
         p.PC11,
@@ -145,6 +200,16 @@ async fn main(spawner: Spawner) -> ! {
     core::write!(&mut s, "Hello DMA World!\r\n",).unwrap();
     unwrap!(usart.write(s.as_bytes()).await);
 
+    if let Err(err) = spawner.spawn(state_machine::state_handler(
+        &CURRENT_STATE,
+        PDU_COMMAND.sender(),
+        &DTI_MPH,
+        &BRAKE_STATE,
+        &TSMS_SENSE,
+    )) {
+        warn!("Could not spawn BMS task: {}", err);
+    }
+
     let mut watchdog = IndependentWatchdog::new(p.IWDG, 4000000);
     watchdog.unleash();
     let mut led_pin = Output::new(p.PC8, Level::Low, Speed::Low);