diff --git a/firmware/src/airbag_control.rs b/firmware/src/airbag_control.rs index 48fc826..bcebd6a 100644 --- a/firmware/src/airbag_control.rs +++ b/firmware/src/airbag_control.rs @@ -5,11 +5,9 @@ //! //! However having it allows clearing all faults, and allows us to extend later to send //! a "crashed" and open contactors in an emergency. -use crate::can_queue; -use crate::car; +use crate::app; use crate::car::Ignition; use crate::dbc::pcan; -use crate::hardware; use crate::hardware::Mono; use fugit::RateExtU32; use hex_literal::hex; @@ -20,15 +18,11 @@ use stm32g4xx_hal::prelude::OutputPin; // Task does two things: // - 1Hz Send CAN message (constant contents) // - 50Hz soft PWM output, 80% high duty for "not crashed", 20% for "crashed" -pub async fn task( - mut pcan_tx: M, - mut car: MCAR, - crash_out: &mut hardware::AcuCrashOutput, -) -> ! -where - M: Mutex>, - MCAR: Mutex, -{ +pub async fn task_airbag_control(cx: app::task_airbag_control::Context<'_>) { + let mut car = cx.shared.car; + let mut pcan_tx = cx.shared.pcan_tx; + let crash_out = cx.local.srs_crash_out; + let airbag_status = pcan::AirbagStatus::try_from(hex!("000000C025029101").as_slice()).unwrap(); let duty_pct = 80; diff --git a/firmware/src/car.rs b/firmware/src/car.rs index 1530b44..d0c5a2a 100644 --- a/firmware/src/car.rs +++ b/firmware/src/car.rs @@ -1,6 +1,6 @@ //! Common state of the entire "car" as presented to the Kona //! components. -use crate::dbc::pcan::{BattHvStatusPrechargeRelay, Messages}; +use crate::dbc::pcan::{BattHvStatusPrechargeRelay, Messages, Vcu200CurrentGear}; use crate::fresh::Fresh; use defmt::Format; use fugit::ExtU32; @@ -22,6 +22,8 @@ pub struct CarState { charge_port_locked: bool, is_braking: bool, + gear: Fresh, + soc_batt: f32, v_batt: f32, i_batt: f32, @@ -60,6 +62,28 @@ pub enum Contactor { Closed, } +#[derive(Clone, Copy, Format, PartialEq)] +pub enum Gear { + Park, + Neutral, + Drive, + Reverse, +} + +impl TryFrom for Gear { + type Error = (); + + fn try_from(value: Vcu200CurrentGear) -> Result { + match value { + Vcu200CurrentGear::P => Ok(Self::Park), + Vcu200CurrentGear::D => Ok(Self::Drive), + Vcu200CurrentGear::N => Ok(Self::Neutral), + Vcu200CurrentGear::R => Ok(Self::Reverse), + Vcu200CurrentGear::_Other(_) => Err(()), + } + } +} + impl CarState { pub fn new() -> Self { // Note this is where all of the stale timeouts for the Fresh values are set @@ -71,6 +95,8 @@ impl CarState { charge_port_locked: false, is_braking: false, + gear: Fresh::new(3.secs()), + soc_batt: 0.0, v_batt: 0.0, i_batt: 0.0, @@ -247,7 +273,15 @@ impl CarState { // as these two have the same "freshness" they could conceivably be merged somehow self.v_inverter.set(msg.v_inverter()); self.motor_rpm.set(msg.speed_abs() as u16); - } + }, + Messages::Vcu200(msg) => { + if let Ok(gear) = msg.current_gear().try_into() { + self.gear.set(gear); + } else if self.ignition().ig3_on() { + defmt::warn!("VCU200 message sent invalid gear value {}", + msg.current_gear_raw()); + } + }, _ => (), } } diff --git a/firmware/src/ieb.rs b/firmware/src/ieb.rs index dcf1f9d..fa2d688 100644 --- a/firmware/src/ieb.rs +++ b/firmware/src/ieb.rs @@ -3,14 +3,13 @@ //! Also manages traction control and vehicle stability control messages. Most of this //! is spoofed, the VCU's perspective should be that it's forever driving in a straight //! line down a road with perfect traction... -use crate::can_queue; +use crate::app; use crate::can_utils::byte_checksum_simple; use crate::car::{CarState, Ignition}; use crate::dbc::pcan::{ Ieb2a2, Ieb331, Ieb386Wheel, Ieb387Wheel, Ieb507Tcs, ParkingBrake, StabilityControl, TractionControlFast, TractionControlMed, }; -use crate::hardware; use crate::hardware::Mono; use crate::repeater::{Period, Repeater}; use fugit::ExtU32; @@ -18,11 +17,10 @@ use hex_literal::hex; use rtic::Mutex; use rtic_monotonics::Monotonic; -pub async fn task_ieb(mut pcan_tx: MPCAN, mut car: MCAR) -> ! -where - MPCAN: Mutex>, - MCAR: Mutex, -{ +pub async fn task_ieb(cx: app::task_ieb::Context<'_>) { + let mut car = cx.shared.car; + let mut pcan_tx = cx.shared.pcan_tx; + // Initialise all the raw CAN messages let ieb507 = Ieb507Tcs::try_from(hex!("00000001").as_slice()).unwrap(); diff --git a/firmware/src/igpm.rs b/firmware/src/igpm.rs index cd63264..8247d4c 100644 --- a/firmware/src/igpm.rs +++ b/firmware/src/igpm.rs @@ -3,7 +3,7 @@ //! //! Some of these messages may originate from other modules in the car, and be //! forwarded onto the PCAN bus by the IGPM. Others originate from the IGPM. -use crate::can_queue::Tx; +use crate::app; use crate::car::{self, CarState, Contactor, Ignition}; use crate::dbc::pcan::{ BodyState, BodyStateDrvDoorSw, BodyStateDrvSeatBeltSw, BodyStateIgnitionSw, @@ -11,17 +11,16 @@ use crate::dbc::pcan::{ Cgw561, Cgw578, Cgw588, Cgw5b3, Cgw5b3PowerState, Cgw5b3UnkPowerRelated, Cgw5df, ChargePort, ChargeSettings, ChargeSettingsAcChargingCurrent, Clock, Odometer, Steering, }; -use crate::hardware::{self, Mono}; +use crate::hardware::Mono; use crate::repeater::{Period, Repeater}; use hex_literal::hex; use rtic::Mutex; use rtic_monotonics::Monotonic; -pub async fn task_igpm(mut pcan_tx: MPCAN, mut car: MCAR) -> ! -where - MPCAN: Mutex>, - MCAR: Mutex, -{ +pub async fn task_igpm(cx: app::task_igpm::Context<'_>) { + let mut pcan_tx = cx.shared.pcan_tx; + let mut car = cx.shared.car; + let charge_settings = ChargeSettings::new(ChargeSettingsAcChargingCurrent::Maximum.into()).unwrap(); diff --git a/firmware/src/lib.rs b/firmware/src/lib.rs deleted file mode 100644 index e54cac4..0000000 --- a/firmware/src/lib.rs +++ /dev/null @@ -1,49 +0,0 @@ -#![no_main] -#![no_std] -#![feature(never_type)] - -use core::sync::atomic::{AtomicUsize, Ordering}; -use defmt_brtt as _; // global logger - -use panic_probe as _; - -use stm32g4xx_hal as _; // memory layout - -pub mod airbag_control; -pub mod can_queue; -pub mod can_utils; -pub mod car; -pub mod dbc; -pub mod repeater; -pub mod fresh; -pub mod hardware; -pub mod ieb; -pub mod igpm; - -// Make some common type aliases for fugit Duration, Instance and Rate -// based on our firmware's 1ms tick period -type Duration = fugit::Duration; -type Instant = fugit::Instant; -type Rate = fugit::Rate; - -// same panicking *behavior* as `panic-probe` but doesn't print a panic message -// this prevents the panic message being printed *twice* when `defmt::panic` is invoked -#[defmt::panic_handler] -fn panic() -> ! { - cortex_m::asm::udf() -} - -static COUNT: AtomicUsize = AtomicUsize::new(0); -defmt::timestamp!("{=usize}", { - // NOTE(no-CAS) `timestamps` runs with interrupts disabled - let n = COUNT.load(Ordering::Relaxed); - COUNT.store(n + 1, Ordering::Relaxed); - n -}); - -/// Terminates the application and makes `probe-rs` exit with exit-code = 0 -pub fn exit() -> ! { - loop { - cortex_m::asm::bkpt(); - } -} diff --git a/firmware/src/bin/fakon.rs b/firmware/src/main.rs similarity index 76% rename from firmware/src/bin/fakon.rs rename to firmware/src/main.rs index ae84001..6c0ebf0 100644 --- a/firmware/src/bin/fakon.rs +++ b/firmware/src/main.rs @@ -1,14 +1,43 @@ #![no_main] #![no_std] #![feature(type_alias_impl_trait)] +#![feature(never_type)] -use fakon as _; +use core::sync::atomic::{AtomicUsize, Ordering}; +use defmt_brtt as _; // global logger + +use panic_probe as _; + +use stm32g4xx_hal as _; // memory layout + +pub mod airbag_control; +pub mod can_queue; +pub mod can_utils; +pub mod car; +pub mod dbc; +pub mod fresh; +pub mod hardware; +pub mod ieb; +pub mod igpm; +pub mod repeater; + +// Make some common type aliases for fugit Duration, Instance and Rate +// based on our firmware's 1ms tick period +type Duration = fugit::Duration; +type Instant = fugit::Instant; +type Rate = fugit::Rate; #[rtic::app( device = stm32g4xx_hal::stm32, dispatchers = [USBWAKEUP, COMP1_2_3, COMP4_5_6, COMP7, SAI, I2C4_EV, I2C4_ER] )] mod app { + use crate::can_queue; + use crate::car; + use crate::car::Ignition; + use crate::dbc::pcan; + use crate::hardware; + use crate::hardware::Mono; use debouncr::debounce_stateful_3; use debouncr::debounce_stateful_5; use debouncr::Edge::Falling; @@ -16,13 +45,6 @@ mod app { use defmt::Debug2Format; use embedded_can::Frame; use embedded_can::Id; - use fakon; - use fakon::can_queue; - use fakon::car; - use fakon::car::Ignition; - use fakon::dbc::pcan; - use fakon::hardware; - use fakon::hardware::Mono; use fugit::ExtU32; use rtic_monotonics::Monotonic; use stm32g4xx_hal::hal::digital::v2::OutputPin; @@ -68,9 +90,9 @@ mod app { pcan_rx::spawn().unwrap(); poll_slow_inputs::spawn().unwrap(); - airbag_control::spawn().unwrap(); - ieb::spawn().unwrap(); - igpm::spawn().unwrap(); + task_airbag_control::spawn().unwrap(); + task_ieb::spawn().unwrap(); + task_igpm::spawn().unwrap(); log_info::spawn().unwrap(); ( @@ -128,24 +150,20 @@ mod app { } } - #[task(shared = [pcan_tx, car], local = [srs_crash_out], priority = 3)] - async fn airbag_control(cx: airbag_control::Context) { - fakon::airbag_control::task( - cx.shared.pcan_tx, - cx.shared.car, - cx.local.srs_crash_out, - ) - .await; - } + use crate::airbag_control::task_airbag_control; + use crate::ieb::task_ieb; + use crate::igpm::task_igpm; - #[task(shared = [pcan_tx, car], priority = 3)] - async fn ieb(cx: ieb::Context) { - fakon::ieb::task_ieb(cx.shared.pcan_tx, cx.shared.car).await; - } + // Task declarations all extern-ed to split the firmware up into modules + extern "Rust" { + #[task(shared = [pcan_tx, car], local = [srs_crash_out], priority = 3)] + async fn task_airbag_control(cx: task_airbag_control::Context); - #[task(shared = [pcan_tx, car], priority = 3)] - async fn igpm(cx: igpm::Context) { - fakon::igpm::task_igpm(cx.shared.pcan_tx, cx.shared.car).await; + #[task(shared = [pcan_tx, car], priority = 3)] + async fn task_ieb(cx: task_ieb::Context); + + #[task(shared = [pcan_tx, car], priority = 3)] + async fn task_igpm(cx: task_igpm::Context); } // FDCAN_INTR0_IT and FDCAN_INTR1_IT are swapped, until stm32g4 crate @@ -230,3 +248,25 @@ mod app { } } } + +// same panicking *behavior* as `panic-probe` but doesn't print a panic message +// this prevents the panic message being printed *twice* when `defmt::panic` is invoked +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} + +static COUNT: AtomicUsize = AtomicUsize::new(0); +defmt::timestamp!("{=usize}", { + // NOTE(no-CAS) `timestamps` runs with interrupts disabled + let n = COUNT.load(Ordering::Relaxed); + COUNT.store(n + 1, Ordering::Relaxed); + n +}); + +/// Terminates the application and makes `probe-rs` exit with exit-code = 0 +pub fn exit() -> ! { + loop { + cortex_m::asm::bkpt(); + } +}