Merge branch 'working-fin-board-can'

tomcru4 · tomcru4 · commit b67392063bd8 · 2025-10-01T15:02:13.000+02:00
diff --git a/firmwares/fancyfins/Cargo.lock b/firmwares/fancyfins/Cargo.lock
diff --git a/firmwares/fancyfins/Cargo.toml b/firmwares/fancyfins/Cargo.toml
@@ -34,3 +34,4 @@ shared_types = { path = "../../shared_types" }
 w25q = "0.2.9"
 smart-leds = "0.4.0"
 ws2812-spi = "0.5.0"
+heapless = "0.8"
diff --git a/firmwares/fancyfins/src/animations.rs b/firmwares/fancyfins/src/animations.rs
@@ -0,0 +1,32 @@
+use defmt::info;
+use smart_leds::RGB8;
+
+/// This is unique to Wirth leds on the Hyacinth FancyFin hardware.
+pub const GREEN: RGB8 = RGB8::new(0, 255, 0);
+pub const RED: RGB8 = RGB8::new(255, 0, 0);
+pub const BLUE: RGB8 = RGB8::new(0, 0, 255);
+pub const WHITE: RGB8 = RGB8::new(255, 255, 255);
+pub const BLACK: RGB8 = RGB8::new(0, 0, 0);
+
+///  Takes a percentage and lights up the leds up to (but not including) this part.
+/// Other leds will be turned off.
+pub fn mk_fill_percentage(percentage: u8, color: RGB8, buffer: &mut [RGB8], background: Option<RGB8>) {
+    info!("mk_fill_percentage: {}", percentage);
+    let percentage = percentage.clamp(0, 100);
+    let top_position = (percentage as usize * buffer.len()) / 100;
+    for i in 0..top_position {
+        buffer[led_position_to_index(i as u32) as usize] = color;
+    }
+    if let Some(background) = background {
+        for i in top_position..buffer.len() {
+            buffer[led_position_to_index(i as u32) as usize] = background;
+        }
+    }
+}
+
+/// Takes the position of the led counted from the bottom up, and returns the index in the chain.
+/// This is unique to Hyacinth FancyFin hardware.
+fn led_position_to_index(position: u32) -> u32 {
+    let half = position / 2;
+    10 + half - (position % 2) * (2 * half + 1)
+}
diff --git a/firmwares/fancyfins/src/can.rs b/firmwares/fancyfins/src/can.rs
@@ -1,33 +1,32 @@
 use defmt::*;
 use embassy_executor::Spawner;
-use embassy_stm32::can::bxcan::{filter, Fifo, Frame, Id, StandardId};
+use embassy_stm32::can::bxcan::{Data, Fifo, Frame, Id, StandardId, filter};
 use embassy_stm32::can::{Can, CanRx, CanTx};
 use embassy_stm32::peripherals::CAN;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 
 use embassy_sync::pubsub::{PubSubChannel, Publisher, Subscriber};
-use shared_types::{CanBusMessage, CanBusMessageId, FlightMode, TelemetryToPayloadMessage};
+use shared_types::FlightMode;
+//use shared_types::{CanBusMessage, CanBusMessageId, FlightMode, TelemetryToPayloadMessage};
+use heapless::Vec;
 use static_cell::StaticCell;
 
+use shared_types::can::{
+    CanMessage,
+    structure::{Can2aFrame, CanFrameId},
+};
+
 pub const CAN_QUEUE_SIZE: usize = 3; // TODO
 pub const NUM_CAN_SUBSCRIBERS: usize = 1; // TODO
 pub const NUM_CAN_PUBLISHERS: usize = 1; // TODO
 
-pub type CanFrame = (u16, [u8; 8]);
-pub type CanInChannel = PubSubChannel<CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
-pub type CanInSubscriper =
-    Subscriber<'static, CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
-pub type CanOutChannel = PubSubChannel<CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
-pub type CanOutPublisher = Publisher<'static, CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
-
-pub static CAN_IN: StaticCell<CanInChannel> = StaticCell::new();
-pub static CAN_OUT: StaticCell<CanOutChannel> = StaticCell::new();
+pub type CanRxCh = PubSubChannel<CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
+pub type CanRxSub = Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>;
+pub type CanTxCh = PubSubChannel<CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
+pub type CanTxPub = Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>;
 
-pub const NUM_FLIGHT_MODE_SUBSCRIBERS: usize = 3;
-pub type FlightModeChannel = PubSubChannel<CriticalSectionRawMutex, FlightMode, 1, NUM_FLIGHT_MODE_SUBSCRIBERS, 1>;
-pub type FlightModeSubscriber =
-    Subscriber<'static, CriticalSectionRawMutex, FlightMode, 1, NUM_FLIGHT_MODE_SUBSCRIBERS, 1>;
-pub static FLIGHT_MODE: StaticCell<FlightModeChannel> = StaticCell::new();
+pub static CAN_IN: StaticCell<CanRxCh> = StaticCell::new();
+pub static CAN_OUT: StaticCell<CanTxCh> = StaticCell::new();
 
 static CAN: StaticCell<Can<'static, CAN>> = StaticCell::new();
 static CAN_TX: StaticCell<CanTx<'static, 'static, CAN>> = StaticCell::new();
@@ -36,8 +35,8 @@ static CAN_RX: StaticCell<CanRx<'static, 'static, CAN>> = StaticCell::new();
 pub async fn spawn(
     mut can: Can<'static, CAN>,
     spawner: Spawner,
-    publisher: Publisher<'static, CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
-    subscriber: Subscriber<'static, CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
+    publisher: Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
+    subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
 ) {
     can.modify_config()
         .set_loopback(false)
@@ -46,8 +45,9 @@ pub async fn spawn(
         .leave_disabled();
 
     let telemetry_filter = filter::Mask32::frames_with_std_id(
-        StandardId::new(CanBusMessageId::TelemetryBroadcast(0).into()).unwrap(),
-        StandardId::new(0x700).unwrap(),
+        //StandardId::new(CanBusMessageId::TelemetryBroadcast(0).into()).unwrap(),
+        StandardId::new(0x1af).unwrap(),
+        StandardId::new(0).unwrap(),
     );
 
     can.modify_filters().enable_bank(0, Fifo::Fifo0, telemetry_filter);
@@ -67,15 +67,15 @@ pub async fn spawn(
 #[embassy_executor::task]
 async fn run_tx(
     can_tx: &'static mut CanTx<'static, 'static, CAN>,
-    mut subscriber: Subscriber<'static, CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
+    mut subscriber: Subscriber<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, 1, NUM_CAN_PUBLISHERS>,
 ) -> ! {
     loop {
-        let (address, data) = subscriber.next_message_pure().await;
-        let Some(sid) = StandardId::new(address) else {
+        let msg: Can2aFrame = subscriber.next_message_pure().await.into();
+        let Some(sid) = StandardId::new(msg.id.into()) else {
             continue;
         };
 
-        let frame = Frame::new_data(sid, data);
+        let frame = Frame::new_data(sid, Data::new(msg.payload.as_slice()).unwrap());
         can_tx.write(&frame).await;
         can_tx.flush_all().await;
     }
@@ -84,46 +84,41 @@ async fn run_tx(
 #[embassy_executor::task]
 async fn run_rx(
     can_rx: &'static mut CanRx<'static, 'static, CAN>,
-    publisher: Publisher<'static, CriticalSectionRawMutex, CanFrame, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
+    publisher: Publisher<'static, CriticalSectionRawMutex, CanMessage, CAN_QUEUE_SIZE, NUM_CAN_SUBSCRIBERS, 1>,
 ) -> ! {
     loop {
         match can_rx.read().await {
             Ok(envelope) => {
                 let frame = envelope.frame;
-                let Some(data) = frame.data() else {
+                let Id::Standard(id) = frame.id() else {
+                    warn!("Unexpected extended 29bit Can Frame Id, dropping.");
                     continue;
                 };
-
-                let Id::Standard(sid) = frame.id() else {
-                    // EID package, skipping.
+                let Some(frame_data) = frame.data() else {
+                    warn!("Unexpected non-data Can Frame, dropping.");
                     continue;
                 };
-                let id_raw = sid.as_raw();
 
-                let Ok(data_array) = data.as_ref().try_into() else {
-                    // frame wasn't 8 bytes long, skip.
+                let Ok(data) = Vec::from_slice(frame_data) else {
+                    warn!("Unexpected Can Frame data of lenght > 8 bytes, dropping.");
                     continue;
                 };
-
-                publisher.publish_immediate((id_raw, data_array));
+                let Ok(id) = CanFrameId::try_from(id.as_raw()) else {
+                    warn!(
+                        "Can Frame id could not be mapped to a type, either the sender's or this software might be outdated. id: {}",
+                        id.as_raw()
+                    );
+                    continue;
+                };
+                let message = Can2aFrame { id, payload: data };
+                let Ok(message_parsed) = CanMessage::try_from(message) else {
+                    warn!("Malformed Can Message payload, message could not be parsed into type, dropping.");
+                    continue;
+                };
+                publisher.publish_immediate(message_parsed); // TODO: think about publish immediate
             }
             Err(e) => {
-                error!("Failed to read envelope: {:?}", Debug2Format(&e))
-            }
-        }
-    }
-}
-
-#[embassy_executor::task]
-pub async fn run_flight_mode_listener(
-    mut can_subscriber: CanInSubscriper,
-    flight_mode_publisher: Publisher<'static, CriticalSectionRawMutex, FlightMode, 1, NUM_FLIGHT_MODE_SUBSCRIBERS, 1>,
-) -> ! {
-    loop {
-        let (sid, data) = can_subscriber.next_message_pure().await;
-        if sid == CanBusMessageId::TelemetryBroadcast(0).into() {
-            if let Ok(Some(msg)) = TelemetryToPayloadMessage::parse(data) {
-                flight_mode_publisher.publish_immediate(msg.mode);
+                error!("Can Bus Error: Failed to read can envelope: {:?}", Debug2Format(&e))
             }
         }
     }
diff --git a/firmwares/fancyfins/src/leds.rs b/firmwares/fancyfins/src/leds.rs
@@ -1,14 +1,18 @@
-use defmt::error;
-use embassy_executor;
+use defmt::*;
 use embassy_stm32::peripherals::*;
-use embassy_stm32::spi::{Config, Phase, Polarity, Spi};
+use embassy_stm32::spi::{Config, Phase, Polarity};
 use embassy_stm32::time::Hertz;
 use embassy_time::{Duration, Timer};
 use shared_types::FlightMode;
-use smart_leds::SmartLedsWrite;
+use shared_types::can::{CanMessage, TelemetryBroadcast};
 use smart_leds::RGB8;
+use smart_leds::SmartLedsWrite;
 use ws2812_spi::Ws2812;
 
+use animations::{BLACK, BLUE, GREEN, RED, WHITE};
+
+use crate::animations::{self, mk_fill_percentage};
+
 const NAVIGATION_LIGHT_PATTERN: [RGB8; 16] = [
     RGB8 {
         r: 0xff,
@@ -93,47 +97,53 @@ const NAVIGATION_LIGHT_PATTERN: [RGB8; 16] = [
 ];
 const NAVIGATION_LIGHT_BLINK_DURATION_MILLIS: u64 = 50;
 
-async fn boot_animation(leds: &mut Ws2812<Spi<'static, SPI3, DMA1_CH1, DMA1_CH2>>) {
-    let mut colors = [RGB8::default(); 16];
-
-    for i in 0..16 {
-        Timer::after(Duration::from_millis(20)).await;
-        colors[i] = RGB8 {
-            r: 0x00,
-            g: 0xff,
-            b: 0x00,
-        };
-        let _ = leds.write(colors);
+async fn boot_animation(
+    buffer: &mut [RGB8],
+    led_lightup_time: Duration,
+    color: RGB8,
+    mut update_cb: impl FnMut(&[RGB8]),
+) {
+    for i in 0..buffer.len() {
+        mk_fill_percentage(((100 * i) / buffer.len()) as u8, color, buffer, Some(BLACK));
+        update_cb(buffer);
+        Timer::after(led_lightup_time).await;
     }
-
     Timer::after(Duration::from_millis(200)).await;
-    let _ = leds.write([RGB8::default(); 16]);
+    mk_fill_percentage(100, BLACK, buffer, Some(BLACK));
+    update_cb(buffer);
     Timer::after(Duration::from_millis(200)).await;
 }
 
 #[embassy_executor::task]
 pub async fn run(
-    mut flight_mode_subscriber: crate::can::FlightModeSubscriber,
+    mut can_rx_sub: crate::can::CanRxSub,
     spi: SPI3,
     led_signal_pin: PB5,
     dma_out: DMA1_CH1,
     dma_in: DMA1_CH2,
 ) -> ! {
-    let mut flight_mode = FlightMode::default();
-
+    // spi settings
     let mut config = Config::default();
-    config.frequency = Hertz::khz(3500);
+    config.frequency = Hertz::khz(2400);
     config.mode.polarity = Polarity::IdleLow;
     config.mode.phase = Phase::CaptureOnSecondTransition;
     let spi_bus = embassy_stm32::spi::Spi::new_txonly_nosck(spi, led_signal_pin, dma_out, dma_in, config);
 
     let mut leds = Ws2812::new(spi_bus);
+    let mut flight_mode = FlightMode::default();
+    let mut buffer: [RGB8; 20] = [BLACK; 20];
 
-    boot_animation(&mut leds).await;
+    boot_animation(&mut buffer, Duration::from_millis(20), GREEN, |b: &[RGB8]| {
+        // TODO: get rid of ocpied
+        let _ = leds.write(b.iter().copied());
+    })
+    .await;
 
     loop {
-        while let Some(new_fm) = flight_mode_subscriber.try_next_message_pure() {
-            flight_mode = new_fm;
+        while let Some(msg) = can_rx_sub.try_next_message_pure() {
+            if let CanMessage::Telem(TelemetryBroadcast::FlightMode(new_fm)) = msg {
+                flight_mode = new_fm;
+            }
         }
 
         if flight_mode >= FlightMode::Burn {
diff --git a/firmwares/fancyfins/src/main.rs b/firmwares/fancyfins/src/main.rs
@@ -2,20 +2,21 @@
 #![no_main]
 
 use embassy_executor::Spawner;
+use embassy_stm32::Config;
 use embassy_stm32::adc::Adc;
 use embassy_stm32::gpio::Input;
 use embassy_stm32::peripherals::*;
 use embassy_stm32::time::Hertz;
 use embassy_stm32::wdg::IndependentWatchdog;
-use embassy_stm32::Config;
 use embassy_sync::pubsub::PubSubChannel;
 use embassy_time::{Delay, Duration, Ticker};
 
 use {defmt_rtt as _, panic_probe as _};
 
+mod animations;
 mod can;
 mod leds;
-mod strain_gauge;
+//mod strain_gauge;
 
 use can::*;
 
@@ -50,17 +51,6 @@ async fn main(low_priority_spawner: Spawner) {
     config.rcc.adcclk = Some(Hertz::mhz(14));
     let p = embassy_stm32::init(config);
 
-    // Remap CAN to be on PB8/9
-    embassy_stm32::pac::AFIO.mapr().modify(|w| w.set_can1_remap(2));
-
-    let addr0 = Input::new(p.PA8, embassy_stm32::gpio::Pull::Up);
-    let addr1 = Input::new(p.PA9, embassy_stm32::gpio::Pull::Up);
-    let address = (addr0.is_low() as u8) << 1 + (addr1.is_low() as u8);
-    // Fin #1 has a broken jumper, so hardcode this when flashing.
-    //let address = 1;
-
-    defmt::info!("Fin #{} startup", address);
-
     // Start watchdog
     let mut iwdg = IndependentWatchdog::new(p.IWDG, 512_000); // 512ms timeout
     iwdg.unleash();
@@ -69,36 +59,30 @@ async fn main(low_priority_spawner: Spawner) {
     let can_in = CAN_IN.init(PubSubChannel::new());
     let can_out = CAN_OUT.init(PubSubChannel::new());
     // Can RX on PB8 can TX on PB9
-    let can = embassy_stm32::can::Can::new(p.CAN, p.PB8, p.PB9, Irqs);
+    let can = embassy_stm32::can::Can::new(p.CAN, p.PA11, p.PA12, Irqs);
 
     // Start main CAN RX/TX tasks
     can::spawn(can, low_priority_spawner, can_in.publisher().unwrap(), can_out.subscriber().unwrap()).await;
 
-    // Run a task which listens to CAN messages and publishes flight mode information
-    let flight_mode = FLIGHT_MODE.init(PubSubChannel::new());
-    low_priority_spawner
-        .spawn(can::run_flight_mode_listener(can_in.subscriber().unwrap(), flight_mode.publisher().unwrap()))
-        .unwrap();
-
     // Run LED task
     low_priority_spawner
-        .spawn(leds::run(flight_mode.subscriber().unwrap(), p.SPI3, p.PB5, p.DMA1_CH1, p.DMA1_CH2))
+        .spawn(leds::run(can_in.subscriber().unwrap(), p.SPI3, p.PB5, p.DMA1_CH1, p.DMA1_CH2))
         .unwrap();
 
-    // Run strain gauge task. TODO: split flash into separate task?
-    let mut adc = Adc::new(p.ADC1, &mut Delay);
-    adc.set_sample_time(embassy_stm32::adc::SampleTime::Cycles239_5);
-
-    low_priority_spawner
-        .spawn(strain_gauge::run(
-            address,
-            adc,
-            p.PC0,
-            p.PC1,
-            p.PC2,
-            p.PC3,
-            flight_mode.subscriber().unwrap(),
-            can_out.publisher().unwrap(),
-        ))
-        .unwrap();
+    // Run strain gauge task.
+    // let mut adc = Adc::new(p.ADC1, &mut Delay);
+    // adc.set_sample_time(embassy_stm32::adc::SampleTime::Cycles239_5);
+
+    //low_priority_spawner
+    //    .spawn(strain_gauge::run(
+    //        address,
+    //        adc,
+    //        p.PC0,
+    //        p.PC1,
+    //        p.PC2,
+    //        p.PC3,
+    //        flight_mode.subscriber().unwrap(),
+    //        can_out.publisher().unwrap(),
+    //    ))
+    //    .unwrap();
 }
diff --git a/shared_types/src/can/structure.rs b/shared_types/src/can/structure.rs
