diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index a34a89254..58dfd7ea9 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -16,9 +16,14 @@ jobs: steps: - uses: actions/checkout@v3 +<<<<<<< HEAD - name: Build & Lint run: rustup target add thumbv7em-none-eabihf && cd app && cargo clippy -- -D warnings +======= + - name: Build + run: rustup target add thumbv7em-none-eabihf && cd app && cargo build +>>>>>>> 441ec3870e99a4435b7c6742938ceacc8c9d3b10 station: @@ -33,8 +38,13 @@ jobs: sudo apt-get install -y libwebkit2gtk-4.0-dev libwebkit2gtk-4.1-dev libappindicator3-dev librsvg2-dev patchelf libgtk-3-dev export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:$PKG_CONFIG_PATH +<<<<<<< HEAD - name: Build & Lint run: cd gs/station && cargo clippy --features full -- -D warnings +======= + - name: Build + run: cd gs/station && cargo build --features full +>>>>>>> 441ec3870e99a4435b7c6742938ceacc8c9d3b10 - name: Test run: cd gs/station && cargo test --features full diff --git a/app/.cargo/config.toml b/app/.cargo/config.toml index acc7acb4b..45219d0ba 100644 --- a/app/.cargo/config.toml +++ b/app/.cargo/config.toml @@ -7,4 +7,4 @@ target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) [env] #DEFMT_LOG = "off" -DEFMT_LOG = "trace" +DEFMT_LOG = "error" diff --git a/app/build.rs b/app/build.rs index 96e92f188..49188d264 100644 --- a/app/build.rs +++ b/app/build.rs @@ -1,13 +1,15 @@ #![allow(non_snake_case)] +use anyhow::anyhow; extern crate serde; +use std::collections::BTreeMap; use std::env; use std::fs; use std::path::Path; use anyhow::Result; -use goose_utils::check_ids; +use goose_utils::check_config; use goose_utils::ip::configure_gs_ip; use serde::Deserialize; @@ -28,7 +30,6 @@ struct GS { ip: [u8; 4], force: bool, port: u16, - // udp_port: u16, buffer_size: usize, timeout: u64, heartbeat: u64, @@ -38,21 +39,22 @@ struct GS { struct Pod { net: NetConfig, internal: InternalConfig, - bms: Bms, + comm: Comm, + heartbeats: BTreeMap, } #[derive(Debug, Deserialize)] -struct Bms { - lv_ids: Vec, - hv_ids: Vec, +struct Comm { + bms_lv_ids: Vec, + bms_hv_ids: Vec, gfd_ids: Vec, } #[derive(Debug, Deserialize)] struct NetConfig { - ip: [u8; 4], - port: u16, - udp_port: u16, + // ip: [u8; 4], + // port: u16, + // udp_port: u16, mac_addr: [u8; 6], keep_alive: u64, } @@ -70,12 +72,6 @@ pub const COMMANDS_PATH: &str = "../config/commands.toml"; pub const EVENTS_PATH: &str = "../config/events.toml"; fn main() -> Result<()> { - // if cfg!(debug_assertions) { - // env::set_var("DEFMT_LOG", "trace"); - // } else { - // env::set_var("DEFMT_LOG", "off"); - // } - let out_dir = env::var("OUT_DIR")?; let dest_path = Path::new(&out_dir).join("config.rs"); @@ -84,16 +80,18 @@ fn main() -> Result<()> { let mut content = String::from("//@generated\n"); - let _ = check_ids(DATATYPES_PATH, COMMANDS_PATH, EVENTS_PATH); + content.push_str(&check_config(DATATYPES_PATH, COMMANDS_PATH, EVENTS_PATH, CONFIG_PATH)?); content.push_str(&configure_ip(&config)); content.push_str(&configure_gs_ip(config.gs.ip, config.gs.port, config.gs.force)?); content.push_str(&configure_pod(&config)); content.push_str(&configure_internal(&config)); content.push_str(&goose_utils::commands::generate_commands(COMMANDS_PATH, true)?); - content.push_str(&goose_utils::datatypes::generate_datatypes(DATATYPES_PATH, false)?); + let dt = goose_utils::datatypes::generate_datatypes(DATATYPES_PATH, false)?; + content.push_str(&dt); content.push_str(&goose_utils::events::generate_events(EVENTS_PATH, true)?); content.push_str(&goose_utils::info::generate_info(CONFIG_PATH, false)?); + content.push_str(&configure_heartbeats(&config, &dt)?); // content.push_str(&*can::main(&id_list)); fs::write(dest_path.clone(), content).unwrap_or_else(|e| { @@ -112,27 +110,41 @@ fn main() -> Result<()> { Ok(()) } +fn configure_heartbeats(config: &Config, dt: &str) -> Result { + let mut x = format!("\npub const HEARTBEATS_LEN: usize = {};\npub const HEARTBEATS: [(Datatype, u64); HEARTBEATS_LEN] = [", config.pod.heartbeats.len()); + for (key, val) in &config.pod.heartbeats { + if !dt.contains(key) { + return Err(anyhow!("\n\nFound heartbeat for non-existing datatype: {:?}\nYou can only add a timeout for datatypes present in /config/datatypes.toml (check your spelling)\n", key)); + } + x.push_str(&format!("(Datatype::{}, {}), ", key, val)); + } + x.push_str("];\n"); + Ok(x) +} + fn configure_ip(config: &Config) -> String { format!("pub const NETWORK_BUFFER_SIZE: usize = {};\n", config.gs.buffer_size) + &*format!("pub const IP_TIMEOUT: u64 = {};\n", config.gs.timeout) } fn configure_pod(config: &Config) -> String { + // format!( + // "pub static POD_IP_ADDRESS: ([u8;4],u16) = ([{},{},{},{}],{});\n", + // config.pod.net.ip[0], + // config.pod.net.ip[1], + // config.pod.net.ip[2], + // config.pod.net.ip[3], + // config.pod.net.port + // ) + // + &*format!( + // "pub static POD_UDP_IP_ADDRESS: ([u8;4],u16) = ([{},{},{},{}],{});\n", + // config.pod.net.ip[0], + // config.pod.net.ip[1], + // config.pod.net.ip[2], + // config.pod.net.ip[3], + // config.pod.net.udp_port + // ) + format!( - "pub static POD_IP_ADDRESS: ([u8;4],u16) = ([{},{},{},{}],{});\n", - config.pod.net.ip[0], - config.pod.net.ip[1], - config.pod.net.ip[2], - config.pod.net.ip[3], - config.pod.net.port - ) + &*format!( - "pub static POD_UDP_IP_ADDRESS: ([u8;4],u16) = ([{},{},{},{}],{});\n", - config.pod.net.ip[0], - config.pod.net.ip[1], - config.pod.net.ip[2], - config.pod.net.ip[3], - config.pod.net.udp_port - ) + &*format!( "pub static POD_MAC_ADDRESS: [u8;6] = [{},{},{},{},{},{}];\n", config.pod.net.mac_addr[0], config.pod.net.mac_addr[1], @@ -140,8 +152,8 @@ fn configure_pod(config: &Config) -> String { config.pod.net.mac_addr[3], config.pod.net.mac_addr[4], config.pod.net.mac_addr[5] - ) + &*format!("pub const KEEP_ALIVE: u64 = {};\n", config.pod.net.keep_alive) - + &*format!("pub const HEARTBEAT: u64 = {};\n", config.gs.heartbeat) + ) + &format!("pub const KEEP_ALIVE: u64 = {};\n", config.pod.net.keep_alive) + + &format!("pub const HEARTBEAT: u64 = {};\n", config.gs.heartbeat) } fn configure_internal(config: &Config) -> String { @@ -150,20 +162,34 @@ fn configure_internal(config: &Config) -> String { + &*format!("pub const CAN_QUEUE_SIZE: usize = {};\n", config.pod.internal.can_queue_size) + &*format!( "pub const LV_IDS: [u16;{}] = [{}];\n", - config.pod.bms.lv_ids.len(), - config.pod.bms.lv_ids.iter().map(|x| x.to_string()).collect::>().join(", ") + config.pod.comm.bms_lv_ids.len(), + config + .pod + .comm + .bms_lv_ids + .iter() + .map(|x| x.to_string()) + .collect::>() + .join(", ") ) + &*format!( "pub const HV_IDS: [u16;{}] = [{}];\n", - config.pod.bms.hv_ids.len(), - config.pod.bms.hv_ids.iter().map(|x| x.to_string()).collect::>().join(", ") + config.pod.comm.bms_hv_ids.len(), + config + .pod + .comm + .bms_hv_ids + .iter() + .map(|x| x.to_string()) + .collect::>() + .join(", ") ) + &*format!( "pub const GFD_IDS: [u16;{}] = [{}];\n", - config.pod.bms.gfd_ids.len(), + config.pod.comm.gfd_ids.len(), config .pod - .bms + .comm .gfd_ids .iter() .map(|x| x.to_string()) @@ -172,14 +198,28 @@ fn configure_internal(config: &Config) -> String { ) + &*format!( "pub const BATTERY_GFD_IDS: [u16;{}] = [{},{},{}];\n", - config.pod.bms.lv_ids.len() - + config.pod.bms.hv_ids.len() - + config.pod.bms.gfd_ids.len(), - config.pod.bms.lv_ids.iter().map(|x| x.to_string()).collect::>().join(", "), - config.pod.bms.hv_ids.iter().map(|x| x.to_string()).collect::>().join(", "), + config.pod.comm.bms_lv_ids.len() + + config.pod.comm.bms_hv_ids.len() + + config.pod.comm.gfd_ids.len(), + config + .pod + .comm + .bms_lv_ids + .iter() + .map(|x| x.to_string()) + .collect::>() + .join(", "), + config + .pod + .comm + .bms_hv_ids + .iter() + .map(|x| x.to_string()) + .collect::>() + .join(", "), config .pod - .bms + .comm .gfd_ids .iter() .map(|x| x.to_string()) diff --git a/app/src/core/communication/can.rs b/app/src/core/communication/can.rs index 4620f0beb..8bafca0db 100644 --- a/app/src/core/communication/can.rs +++ b/app/src/core/communication/can.rs @@ -15,6 +15,8 @@ use crate::core::controllers::battery_controller::ground_fault_detection_voltage use crate::core::controllers::can_controller::CanTwoUtils; use crate::pconfig::bytes_to_u64; use crate::pconfig::id_as_value; +use crate::pconfig::send_event; +use crate::send_data; use crate::CanReceiver; use crate::CanSender; use crate::DataSender; @@ -53,21 +55,18 @@ pub async fn can_receiving_handler( let mut error_counter = 0u64; let mut gfd_counter = 0u64; loop { - // #[cfg(debug_assertions)] match bus.read().await { Ok(envelope) => { error_counter = 0; let (frame, timestamp) = envelope.parts(); - // frame.header().format(); let id = id_as_value(frame.id()); #[cfg(debug_assertions)] - data_sender - .send(Datapoint::new( - Datatype::ReceivedCan, - id as u64, - bytes_to_u64(frame.data()), - )) - .await; + send_data!( + data_sender, + Datatype::ReceivedCan, + id as u64, + bytes_to_u64(frame.data()) + ); #[cfg(debug_assertions)] info!("[CAN ({})] received frame: id={:?} data={:?}", bus_nr, id, frame.data()); if DATA_IDS.contains(&id) { @@ -125,17 +124,17 @@ pub async fn can_receiving_handler( .await; } } else if EVENT_IDS.contains(&id) { - event_sender.send(Event::from_id(id, Some(69420))).await; // since we are never supposed to change the speed through the can bus (and run config is the only event with an actual value), i want a magic number that i can filter out from the run config handler just to make sure the pod doesn't do something stupid + // since we are never supposed to change the speed through the can bus (and run config is the only event with an actual value), i want a magic number that i can filter out from the run config handler just to make sure the pod doesn't do something stupid + send_event(event_sender, Event::from_id(id, Some(69420))); } else { #[cfg(debug_assertions)] info!("[CAN ({})] unknown ID: {:?}", bus_nr, id); - data_sender - .send(Datapoint::new( - Datatype::UnknownCanId, - id as u64, - bytes_to_u64(frame.data()), - )) - .await; + send_data!( + data_sender, + Datatype::UnknownCanId, + id as u64, + bytes_to_u64(frame.data()) + ); } }, Err(e) => { diff --git a/app/src/core/communication/mod.rs b/app/src/core/communication/mod.rs index f2ad62443..29232df7a 100644 --- a/app/src/core/communication/mod.rs +++ b/app/src/core/communication/mod.rs @@ -1,3 +1,5 @@ +use defmt::Formatter; + use crate::Datatype; pub mod can; @@ -33,3 +35,15 @@ impl Datapoint { bytes } } + +impl defmt::Format for Datapoint { + fn format(&self, fmt: Formatter) { + defmt::write!( + fmt, + "Datapoint {{ datatype: {:?}, value: {:?}, timestamp: {:?} }}", + self.datatype, + self.value, + self.timestamp + ) + } +} diff --git a/app/src/core/communication/tcp.rs b/app/src/core/communication/tcp.rs index a676cc532..cb91f50d6 100644 --- a/app/src/core/communication/tcp.rs +++ b/app/src/core/communication/tcp.rs @@ -9,18 +9,26 @@ use embassy_stm32::peripherals::ETH; use embassy_time::Instant; use embassy_time::Timer; use embedded_io_async::Write; +use embedded_io_async::WriteReady; use heapless::Deque; use panic_probe as _; use crate::core::communication::Datapoint; use crate::pconfig::embassy_socket_from_config; +use crate::pconfig::queue_data; +use crate::pconfig::queue_event; +use crate::pconfig::send_event; +use crate::pconfig::ticks; +use crate::send_data; use crate::Command; use crate::DataReceiver; use crate::DataSender; use crate::Datatype; use crate::Event; use crate::EventSender; +use crate::Info; use crate::COMMAND_HASH; +use crate::CONFIG_HASH; use crate::DATA_HASH; use crate::EVENTS_HASH; use crate::GS_IP_ADDRESS; @@ -39,7 +47,7 @@ pub async fn tcp_connection_handler( data_receiver: DataReceiver, data_sender: DataSender, ) -> ! { - let mut last_valid_timestamp = embassy_time::Instant::now().as_millis(); + let mut last_valid_timestamp = Instant::now().as_millis(); // info!("------------------------------------------------ TCP Connection Handler Started! ------------------------------------------"); stack.wait_config_up().await; @@ -52,68 +60,56 @@ pub async fn tcp_connection_handler( // let mut socket: TcpSocket = // TcpSocket::new(stack, unsafe { &mut rx_buffer }, unsafe { &mut tx_buffer }); + let mut rx_buffer: [u8; NETWORK_BUFFER_SIZE] = [0u8; { NETWORK_BUFFER_SIZE }]; + let mut tx_buffer: [u8; NETWORK_BUFFER_SIZE] = [0u8; { NETWORK_BUFFER_SIZE }]; + + let mut socket: TcpSocket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); + let mut buf = [0; { NETWORK_BUFFER_SIZE }]; 'netstack: loop { - // info!("====================================================Connecting to ground station______________________________"); - let mut rx_buffer: [u8; NETWORK_BUFFER_SIZE] = [0u8; { NETWORK_BUFFER_SIZE }]; - let mut tx_buffer: [u8; NETWORK_BUFFER_SIZE] = [0u8; { NETWORK_BUFFER_SIZE }]; - - let mut socket: TcpSocket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); + info!("[netstack] loop starting"); match socket.connect(gs_addr).await { Ok(_) => { - last_valid_timestamp = embassy_time::Instant::now().as_millis(); + last_valid_timestamp = Instant::now().as_millis(); }, Err(e) => { - let d = embassy_time::Instant::now().as_millis() - last_valid_timestamp; + let d = Instant::now().as_millis() - last_valid_timestamp; error!("[tcp:stack] error connecting to gs: {:?} (diff={})", e, d); if d > IP_TIMEOUT { - event_sender.send(Event::ConnectionLossEvent).await; + send_event(event_sender, Event::ConnectionLossEvent); } - Timer::after_millis(500).await; + Timer::after_millis(100).await; continue 'netstack; }, } - event_sender.send(Event::ConnectionEstablishedEvent).await; - data_sender - .send(Datapoint::new(Datatype::CommandHash, COMMAND_HASH, Instant::now().as_ticks())) - .await; - data_sender - .send(Datapoint::new(Datatype::DataHash, DATA_HASH, Instant::now().as_ticks())) - .await; - data_sender - .send(Datapoint::new(Datatype::EventsHash, EVENTS_HASH, Instant::now().as_ticks())) - .await; - // let mut connection = client.connect(gs_addr).await.unwrap(); - // info!("----------------------------------------------------------------Connected to ground station=========================="); + send_event(event_sender, Event::ConnectionEstablishedEvent); - // socket.set_keep_alive(Some(Duration::from_millis(KEEP_ALIVE))); - // socket.set_timeout(Some(Duration::from_millis(IP_TIMEOUT))); + // Handshake + // Exchange hashes of the configuration files + // in order to confirm that the exchanged ids are correct + queue_data(data_sender, Datatype::CommandHash, COMMAND_HASH).await; + queue_data(data_sender, Datatype::EventsHash, EVENTS_HASH).await; + queue_data(data_sender, Datatype::DataHash, DATA_HASH).await; + queue_data(data_sender, Datatype::ConfigHash, CONFIG_HASH).await; - /*// let (mut tcp_reader,mut tcp_writer) = unsafe { socket.split() }; - // spawn the writer task: it will take messages from the channel and send them over the TCP connection - // x.spawn(ground_station_message_dispatcher(tcp_writer, data_receiver.clone())).unwrap();*/ + // Begin relying on the frontend + queue_data(data_sender, Datatype::FrontendHeartbeating, 0).await; - #[cfg(debug_assertions)] - match socket.write(b"aaaaaaaaaaaaaaa0").await { - Ok(_) => { - info!("]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]Data sent successfully") - }, - Err(e) => { - info!(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Failed to send data: {:?}", e) - }, - } let mut parsing_buffer = Deque::::new(); // loop to receive data from the TCP connection 'connection: loop { Timer::after_millis(1).await; - if !socket.may_recv() || !socket.may_send() { + if !socket.may_recv() + || !socket.may_send() + || Instant::now().as_millis() - last_valid_timestamp > IP_TIMEOUT + { error!("[tcp] may_recv: connection closed"); break 'connection; } if socket.can_recv() { - last_valid_timestamp = embassy_time::Instant::now().as_millis(); + last_valid_timestamp = Instant::now().as_millis(); let n = socket.read(&mut buf).await.unwrap_or(420000); if n == 42000 { error!("[tcp] Failed to read from socket"); @@ -121,7 +117,7 @@ pub async fn tcp_connection_handler( } if n == 0 { info!("[tcp] Connection closed by ground station.."); - event_sender.send(Event::ConnectionLossEvent).await; + send_event(event_sender, Event::ConnectionLossEvent); break 'connection; } #[cfg(debug_assertions)] @@ -147,15 +143,15 @@ pub async fn tcp_connection_handler( info!("[tcp] Command received: {:?}", cmd); match cmd { Command::EmergencyBrake(_) => { - event_sender.send(Event::EmergencyBraking).await; + send_event(event_sender, Event::EmergencyBraking); #[cfg(debug_assertions)] info!("[tcp] EmergencyBrake command received!!"); if let Err(e) = socket .write_all( &Datapoint::new( Datatype::Info, - crate::Info::EmergencyBrakeReceived.to_idx(), - embassy_time::Instant::now().as_ticks(), + Info::EmergencyBrakeReceived.to_idx(), + ticks(), ) .as_bytes(), ) @@ -177,83 +173,86 @@ pub async fn tcp_connection_handler( Command::launch(_) => { #[cfg(debug_assertions)] info!("[tcp] Levitate command received"); - event_sender.send(Event::LeviLaunchingEvent).await; + send_event(event_sender, Event::LeviLaunchingEvent); }, Command::land(_) => { #[cfg(debug_assertions)] info!("[tcp] StopLevitating command received"); - event_sender.send(Event::LeviLandingEvent).await; + send_event(event_sender, Event::LeviLandingEvent); }, Command::SetRoute(x) => { #[cfg(debug_assertions)] info!("[tcp] Configure command received"); - event_sender.send(Event::SettingRoute(x)).await; + send_event(event_sender, Event::SettingRoute(x)); }, Command::SetSpeeds(x) => { #[cfg(debug_assertions)] info!("[tcp] Configure command received"); - event_sender.send(Event::SettingSpeeds(x)).await; + send_event(event_sender, Event::SettingSpeeds(x)); }, Command::SetOverrides(x) => { - event_sender.send(Event::SettingOverrides(x)).await; + send_event(event_sender, Event::SettingOverrides(x)); }, Command::SetCurrentSpeed(x) => { #[cfg(debug_assertions)] info!("[tcp] SetCurrentSpeed command received"); - event_sender.send(Event::SetCurrentSpeedCommand(x)).await; + send_event(event_sender, Event::SetCurrentSpeedCommand(x)); }, Command::StartRun(_) => { #[cfg(debug_assertions)] info!("[tcp] Start Run command received"); - event_sender.send(Event::RunStarting).await; + send_event(event_sender, Event::RunStarting); }, Command::ContinueRun(_) => { #[cfg(debug_assertions)] info!("[tcp] Start Run command received"); - event_sender.send(Event::ContinueRunEvent).await; + send_event(event_sender, Event::ContinueRunEvent); }, Command::Shutdown(_) => { #[cfg(debug_assertions)] info!("[tcp] Shutdown command received"); - event_sender.send(Event::ExitEvent).await; + send_event(event_sender, Event::ExitEvent); }, Command::StartHV(_) => { #[cfg(debug_assertions)] info!("[tcp] StartHV command received"); - event_sender.send(Event::TurnOnHVCommand).await; + send_event(event_sender, Event::TurnOnHVCommand); }, Command::StopHV(_) => { #[cfg(debug_assertions)] info!("[tcp] StopHV command received"); - event_sender.send(Event::TurnOffHVCommand).await; - // TODO: no turn off HV exists?? + send_event(event_sender, Event::TurnOffHVCommand); }, Command::DcOn(_) => { #[cfg(debug_assertions)] info!("[tcp] DcOn command received"); - event_sender.send(Event::DcTurnedOn).await; + send_event(event_sender, Event::DcTurnedOn); }, Command::DcOff(_) => { #[cfg(debug_assertions)] info!("[tcp] DcOff command received"); - event_sender.send(Event::DcTurnedOff).await; + send_event(event_sender, Event::DcTurnedOff); }, Command::EmitEvent(e) => { #[cfg(debug_assertions)] info!("[tcp] EmitEvent command received"); - event_sender - .send(Event::from_id((e & 0xFFFF) as u16, Some(69420))) - .await; + send_event( + event_sender, + Event::from_id((e & 0xFFFF) as u16, Some(69420)), + ); + }, + Command::CreateDatapoint(x) => { + send_data!(data_sender, Datatype::from_id(x as u16), x); }, Command::SystemReset(_) => { #[cfg(debug_assertions)] info!("[tcp] SystemReset command received"); - event_sender.send(Event::SystemResetCommand).await; + send_event(event_sender, Event::SystemResetCommand); }, Command::FinishRunConfig(_) => { #[cfg(debug_assertions)] info!("[tcp] FinishRunConfig command received"); - event_sender.send(Event::RunConfigCompleteEvent).await; + send_event(event_sender, Event::RunConfigCompleteEvent); }, Command::Heartbeat(x) => { #[cfg(debug_assertions)] @@ -276,10 +275,13 @@ pub async fn tcp_connection_handler( }, } }, + Command::FrontendHeartbeat(x) => { + send_data!(data_sender, Datatype::FrontendHeartbeating, x); + }, Command::ArmBrakes(_) => { #[cfg(debug_assertions)] info!("[tcp] ArmBrakesCommand command received"); - event_sender.send(Event::ArmBrakesCommand).await; + queue_event(event_sender, Event::ArmBrakesCommand).await; }, _ => {}, // TODO: DELETE THIS } @@ -290,21 +292,34 @@ pub async fn tcp_connection_handler( } } - if let Ok(data) = data_receiver.try_receive() { - let data = data.as_bytes(); - #[cfg(debug_assertions)] - info!("[tcp:mpmc] Sending data: {:?}", data); - match socket.write_all(&data).await { - Ok(_) => { - #[cfg(debug_assertions)] - info!("[tcp:socket] Data written successfully"); - }, - Err(e) => { - error!("[tcp:socket] Failed to write data: {:?}", e); - }, - } - } // the else case is of empty MPMC channel queue, - // which triggers very often, so we ignore it. + match socket.write_ready() { + Ok(x) => { + if x { + if let Ok(data) = data_receiver.try_receive() { + let data = data.as_bytes(); + #[cfg(debug_assertions)] + info!("[tcp:mpmc] Sending data: {:?}", data); + match socket.write_all(&data).await { + Ok(_) => { + #[cfg(debug_assertions)] + info!("[tcp:socket] Data written successfully"); + }, + Err(e) => { + error!("[tcp:socket] Failed to write data: {:?}", e); + }, + } + } // the else case is of empty MPMC channel queue, + // which triggers very often, so we ignore it. + } else { + error!("[tcp] socket not writeable"); + Timer::after_millis(500).await; + } + }, + Err(y) => { + error!("[tcp] error getting socket write status: {:?}", y); + Timer::after_millis(500).await; + }, + } } // if this is reached, it means that the connection was dropped info!("D:"); diff --git a/app/src/core/controllers/battery_controller.rs b/app/src/core/controllers/battery_controller.rs index 5292fec9d..b0b746695 100644 --- a/app/src/core/controllers/battery_controller.rs +++ b/app/src/core/controllers/battery_controller.rs @@ -1,8 +1,9 @@ use defmt::debug; -use defmt::info; +use defmt::trace; use crate::core::communication::Datapoint; use crate::pconfig::bytes_to_u64; +use crate::pconfig::queue_dp; use crate::DataSender; use crate::Datatype; use crate::EventSender; @@ -68,28 +69,15 @@ pub struct GroundFaultDetection {} pub async fn ground_fault_detection_isolation_details( data: &[u8], data_sender: DataSender, - timestamp: u64, + t: u64, ) { let negative_insulation_resistance = ((data[1] as u64) << 8) | (data[0] as u64); - data_sender - .send(Datapoint::new( - Datatype::InsulationNegative, - negative_insulation_resistance, - timestamp, - )) - .await; + queue_dp(data_sender, Datatype::InsulationNegative, negative_insulation_resistance, t).await; let positive_insulation_resistance = ((data[3] as u64) << 8) | (data[2] as u64); - data_sender - .send(Datapoint::new( - Datatype::InsulationPositive, - positive_insulation_resistance, - timestamp, - )) - .await; + queue_dp(data_sender, Datatype::InsulationPositive, positive_insulation_resistance, t).await; + let original_measurement_counter = data[4] as u64 | ((data[5] as u64) << 8); - data_sender - .send(Datapoint::new(Datatype::InsulationOriginal, original_measurement_counter, timestamp)) - .await; + queue_dp(data_sender, Datatype::InsulationOriginal, original_measurement_counter, t).await; } pub async fn ground_fault_detection_voltage_details( @@ -98,7 +86,7 @@ pub async fn ground_fault_detection_voltage_details( timestamp: u64, ) { let hv_voltage = ((data[1] as u64) << 8) | (data[0] as u64); - data_sender.send(Datapoint::new(Datatype::IMDVoltageDetails, hv_voltage, timestamp)).await; + queue_dp(data_sender, Datatype::IMDVoltageDetails, hv_voltage, timestamp).await; } //===============BMS===============// @@ -111,51 +99,37 @@ impl BatteryController { _sender: DataSender, timestamp: u64, ) { - debug!("Here BMS"); + trace!("Here BMS"); match Datatype::from_id(id) { Datatype::DefaultBMSLow | Datatype::DefaultBMSHigh => { self.default_bms_startup_info(data, timestamp).await; - info!("Default BMS"); + debug!("Default BMS"); }, Datatype::BatteryVoltageLow | Datatype::BatteryVoltageHigh => { self.battery_voltage_overall_bms(data, timestamp).await; - info!("Battery Voltage") + debug!("Battery Voltage") }, Datatype::DiagnosticBMSLow | Datatype::DiagnosticBMSHigh => { self.diagnostic_bms(data, timestamp).await; - info!("Diagnostic BMS") + debug!("Diagnostic BMS") }, Datatype::BatteryTemperatureLow | Datatype::BatteryTemperatureHigh => { self.overall_temperature_bms(data, timestamp).await; - info!("Battery Temperature") + debug!("Battery Temperature") }, Datatype::BatteryBalanceLow | Datatype::BatteryBalanceHigh => { self.overall_balancing_status_bms(data, timestamp).await; - info!("Battery Balancing") + debug!("Battery Balancing") }, Datatype::ChargeStateLow | Datatype::ChargeStateHigh => { self.state_of_charge_bms(data, timestamp).await; - info!("Charge State") + debug!("Charge State") }, _x if Datatype::SingleCellTemperatureLow.to_id() == id || (Datatype::SingleCellTemperatureHigh_1.to_id() <= id && Datatype::SingleCellTemperatureHigh_14.to_id() >= id) => { debug!("Individual Temperature"); - // if (id - Datatype::SingleCellTemperatureHigh_1.to_id() != 0) { - // self.receive_single_cell_id = true; - // } - // - // if (self.receive_single_cell_id) { - // self.single_cell_id = 0; - // self.module_buffer = [0; 14]; - // self.current_number_of_cells = 0; - // self.receive_single_cell_id = false; - // } else if (Datatype::SingleCellTemperatureLow.to_id() == id) { - // // self.overall_temperature_bms(&*Self::single_cell_low_process(data).await,timestamp); - // } else { - // self.individual_temperature_bms(data, timestamp).await; - // } if self.number_of_temp >= 13 { self.number_of_temp = 0; let mut i = 0; @@ -183,7 +157,7 @@ impl BatteryController { self.number_of_temp += 1; } - info!("Individual Temperature") + debug!("Individual Temperature") }, _x if Datatype::SingleCellVoltageLow.to_id() == id || (Datatype::SingleCellVoltageHigh_1.to_id() <= id @@ -205,14 +179,6 @@ impl BatteryController { i += 1; } } - // if (id - Datatype::SingleCellVoltageHigh_1.to_id() != 0) { - // self.receive_single_cell_id = true; - // } - // if (self.receive_single_cell_id) { - // self.single_cell_id = 0; - // self.module_buffer = [0; 14]; - // self.current_number_of_cells = 0; - // self.receive_single_cell_id = false; if Datatype::SingleCellVoltageLow.to_id() == id { // self.overall_temperature_bms(&*Self::single_cell_low_process(data).await,timestamp); } else { @@ -225,14 +191,14 @@ impl BatteryController { self.number_of_volt += 1; } - info!("Individual Voltage") + debug!("Individual Voltage") }, Datatype::BatteryEventLow | Datatype::BatteryEventHigh => { self.event_bms(data, timestamp).await; - info!("Battery Event") + debug!("Battery Event") }, x => { - info!("Ignored BMS id: {:?}", x.to_id()); + debug!("Ignored BMS: {:?} (id={:?})", x, x.to_id()); }, } } @@ -241,7 +207,7 @@ impl BatteryController { // let mut msg: u64 = 0; let dt = if self.high_voltage { Datatype::BatteryEventHigh } else { Datatype::BatteryEventLow }; - self.data_sender.send(Datapoint::new(dt, bytes_to_u64(data), timestamp)).await; + queue_dp(self.data_sender, dt, bytes_to_u64(data), timestamp).await; } pub async fn battery_voltage_overall_bms(&mut self, data: &[u8], timestamp: u64) { @@ -274,18 +240,10 @@ impl BatteryController { } else { Datatype::TotalBatteryVoltageLow }; - self.data_sender - .send(Datapoint::new(battery_voltage_dt, avg_cell_voltage, timestamp)) - .await; - self.data_sender - .send(Datapoint::new(battery_voltage_min, min_cell_voltage, timestamp)) - .await; - self.data_sender - .send(Datapoint::new(battery_voltage_max, max_cell_voltage, timestamp)) - .await; - self.data_sender - .send(Datapoint::new(total_battery_voltage_dt, total_pack_voltage, timestamp)) - .await; + queue_dp(self.data_sender, battery_voltage_dt, avg_cell_voltage, timestamp).await; + queue_dp(self.data_sender, battery_voltage_min, min_cell_voltage, timestamp).await; + queue_dp(self.data_sender, battery_voltage_max, max_cell_voltage, timestamp).await; + queue_dp(self.data_sender, total_battery_voltage_dt, total_pack_voltage, timestamp).await; } pub async fn default_bms_startup_info(&mut self, data: &[u8], timestamp: u64) { @@ -307,7 +265,7 @@ impl BatteryController { for (i, &x) in data.iter().enumerate() { msg |= (x as u64) << (i * 8); } - self.data_sender.send(Datapoint::new(dt, msg, timestamp)).await; + queue_dp(self.data_sender, dt, msg, timestamp).await; } pub async fn state_of_charge_bms(&mut self, data: &[u8], timestamp: u64) { @@ -327,11 +285,9 @@ impl BatteryController { Datatype::BatteryEstimatedChargeLow }; - self.data_sender.send(Datapoint::new(battery_current_dt, current, timestamp)).await; - self.data_sender.send(Datapoint::new(charge_state_dt, state_of_charge, timestamp)).await; - self.data_sender - .send(Datapoint::new(estimated_charge_dt, estimated_charge, timestamp)) - .await; + queue_dp(self.data_sender, battery_current_dt, current, timestamp).await; + queue_dp(self.data_sender, charge_state_dt, state_of_charge, timestamp).await; + queue_dp(self.data_sender, estimated_charge_dt, estimated_charge, timestamp).await; } pub async fn overall_temperature_bms(&mut self, data: &[u8], timestamp: u64) { @@ -355,13 +311,12 @@ impl BatteryController { Datatype::BatteryMaxTemperatureLow }; - self.data_sender.send(Datapoint::new(battery_temp_dt, avg_temp, timestamp)).await; - self.data_sender.send(Datapoint::new(battery_temp_min, min_temp, timestamp)).await; - self.data_sender.send(Datapoint::new(battery_temp_max, max_temp, timestamp)).await; + queue_dp(self.data_sender, battery_temp_dt, avg_temp, timestamp).await; + queue_dp(self.data_sender, battery_temp_min, min_temp, timestamp).await; + queue_dp(self.data_sender, battery_temp_max, max_temp, timestamp).await; } - #[allow(dead_code)] - pub async fn individual_temperature_bms(&mut self, data: &[u8], timestamp: u64) { + /*pub async fn individual_temperature_bms(&mut self, data: &[u8], timestamp: u64) { for &x in data.iter() { if self.single_cell_id < 8 { self.module_buffer[self.current_number_of_cells] = x as u64; @@ -380,19 +335,16 @@ impl BatteryController { break; } } - } + }*/ async fn send_module_temp(&mut self, timestamp: u64) { let module_id = self.single_cell_id; let (min_temp, max_temp, avg_temp) = Self::module_data_calculation(self.module_buffer).await; - // min_temp = if (min_temp - 100) < 0 { 0 } else { min_temp - 100 }; - // max_temp = if (max_temp - 100) < 0 { 0 } else { max_temp - 100 }; - // avg_temp = if (avg_temp - 100) < 0 { 0 } else { avg_temp - 100 }; let (avg_temp_dt, min_temp_dt, max_temp_dt) = Self::match_temp(module_id).await; - self.data_sender.send(Datapoint::new(avg_temp_dt, avg_temp, timestamp)).await; - self.data_sender.send(Datapoint::new(min_temp_dt, min_temp, timestamp)).await; - self.data_sender.send(Datapoint::new(max_temp_dt, max_temp, timestamp)).await; + queue_dp(self.data_sender, avg_temp_dt, avg_temp, timestamp).await; + queue_dp(self.data_sender, min_temp_dt, min_temp, timestamp).await; + queue_dp(self.data_sender, max_temp_dt, max_temp, timestamp).await; } async fn send_module_voltage(&mut self, timestamp: u64) { @@ -400,9 +352,9 @@ impl BatteryController { let (min_voltage, max_voltage, avg_voltage) = Self::module_data_calculation(self.module_buffer).await; let (avg_voltage_dt, min_voltage_dt, max_voltage_dt) = Self::match_voltage(module_id).await; - self.data_sender.send(Datapoint::new(avg_voltage_dt, avg_voltage + 200, timestamp)).await; - self.data_sender.send(Datapoint::new(min_voltage_dt, min_voltage + 200, timestamp)).await; - self.data_sender.send(Datapoint::new(max_voltage_dt, max_voltage + 200, timestamp)).await; + queue_dp(self.data_sender, avg_voltage_dt, avg_voltage + 200, timestamp).await; + queue_dp(self.data_sender, min_voltage_dt, min_voltage + 200, timestamp).await; + queue_dp(self.data_sender, max_voltage_dt, max_voltage + 200, timestamp).await; } pub async fn match_temp(id: u16) -> (Datatype, Datatype, Datatype) { @@ -525,7 +477,6 @@ impl BatteryController { (min, max, avg) } - #[allow(dead_code)] pub async fn individual_voltages_bms(&mut self, data: &[u8], timestamp: u64) { for &x in data.iter() { if self.single_cell_id < 8 { @@ -565,8 +516,8 @@ impl BatteryController { } else { Datatype::BatteryMaxBalancingLow }; - self.data_sender.send(Datapoint::new(balancing_dt, avg_cell_balancing, timestamp)).await; - self.data_sender.send(Datapoint::new(balancing_min, min_cell_balancing, timestamp)).await; - self.data_sender.send(Datapoint::new(balancing_max, max_cell_balancing, timestamp)).await; + queue_dp(self.data_sender, balancing_dt, avg_cell_balancing, timestamp).await; + queue_dp(self.data_sender, balancing_min, min_cell_balancing, timestamp).await; + queue_dp(self.data_sender, balancing_max, max_cell_balancing, timestamp).await; } } diff --git a/app/src/core/controllers/breaking_controller.rs b/app/src/core/controllers/breaking_controller.rs index 34ccc7d9d..12933241c 100644 --- a/app/src/core/controllers/breaking_controller.rs +++ b/app/src/core/controllers/breaking_controller.rs @@ -12,7 +12,8 @@ use embassy_time::Duration; use embassy_time::Instant; use embassy_time::Timer; -use crate::core::communication::Datapoint; +use crate::pconfig::queue_event; +use crate::send_data; use crate::try_spawn; use crate::DataSender; use crate::Datatype; @@ -35,25 +36,15 @@ pub async fn control_braking_heartbeat( data_sender: DataSender, mut braking_signal: Output<'static>, ) { - // pub async fn control_braking_heartbeat(sender: EventSender, mut braking_heartbeat: SimplePwm<'static, TIM16>) { info!("----------------- Start Braking Heartbeat! -----------------"); let mut booting = true; let mut last_timestamp = Instant::now(); loop { Timer::after_micros(10).await; - // if adc.read(&mut pf12) > 30000 { - // braking_signal.set_low(); - // // info!("------------ BRAKE ! ------------"); - // } else if unsafe { !BRAKE } { braking_signal.set_high(); - // braking_heartbeat.set_duty(Channel::Ch1, braking_heartbeat.get_max_duty()/2); } else { braking_signal.set_low(); - - // braking_heartbeat.set_duty(Channel::Ch1, 0); - // sender.send(Event::EmergencyBrakeCommand).await; - // info!("------------ BRAKE ! ------------"); } if booting { sender.send(Event::BootingCompleteEvent).await; @@ -61,44 +52,12 @@ pub async fn control_braking_heartbeat( } if Instant::now().duration_since(last_timestamp) > Duration::from_millis(1000) { match braking_signal.get_output_level() { - Level::Low => { - data_sender - .send(Datapoint::new( - Datatype::BrakingSignalDebug, - 0, - Instant::now().as_ticks(), - )) - .await; - }, - Level::High => { - data_sender - .send(Datapoint::new( - Datatype::BrakingSignalDebug, - 1, - Instant::now().as_ticks(), - )) - .await; - }, + Level::Low => send_data!(data_sender, Datatype::BrakingSignalDebug, 0), + Level::High => send_data!(data_sender, Datatype::BrakingSignalDebug, 1), } match unsafe { BRAKE } { - true => { - data_sender - .send(Datapoint::new( - Datatype::BrakingBoolDebug, - 1, - Instant::now().as_ticks(), - )) - .await; - }, - false => { - data_sender - .send(Datapoint::new( - Datatype::BrakingBoolDebug, - 0, - Instant::now().as_ticks(), - )) - .await; - }, + true => send_data!(data_sender, Datatype::BrakingBoolDebug, 1), + false => send_data!(data_sender, Datatype::BrakingBoolDebug, 0), } last_timestamp = Instant::now(); } @@ -120,7 +79,7 @@ async fn read_braking_communication( if edge && is_activated { edge = false; // braking comm value is low, so we don't brake until it goes high again if unsafe { !DISABLE_BRAKING_COMMUNICATION } { - event_sender.send(Event::EmergencyBraking).await; + queue_event(event_sender, Event::EmergencyBraking).await; } Timer::after_millis(1000).await; } @@ -130,13 +89,7 @@ async fn read_braking_communication( } Timer::after_micros(10).await; if Instant::now().duration_since(last_timestamp) > Duration::from_millis(500) { - data_sender - .send(Datapoint::new( - Datatype::BrakingCommDebug, - v as u64, - Instant::now().as_ticks(), - )) - .await; + send_data!(data_sender, Datatype::BrakingCommDebug, v as u64); last_timestamp = Instant::now(); } } @@ -159,27 +112,7 @@ impl BrakingController { // If we want to keep it alive we send a 10khz digital clock signal let braking_rearm: Output = Output::new(pg1, Level::High, Speed::Low); // <--- To keep the breaks not rearmed we send a 1, if we want to arm the breaks we send a 0 - //let mut led: Output = Output::new(pb0, Level::High, Speed::Low); - // let mut led2 : Output = Output::new(pd5,Level::High,Speed::Low); - //led.set_high(); - //info!("set led on pb0 to high"); - // let mut pwm = SimplePwm::new( - // ptime, - // Some(PwmPin::new_ch1(pb8, OutputType::PushPull)), - // None, - // None, - // None, - // khz(30), - // Default::default(), - // ); - // let braking_communication = Input::new(pf12, Pull::None); // <--- If its HIGH it means that breaks are rearmed, if its low it , means we are breaking - // Finally if we set the heartbeat to LOW, and we still receive a 1 is basically means we are crashing so lets actually make use of the crashing state - - // let mut braking_communication = Adc::new(); - // braking_communication - let braking_signal = Output::new(pb8, Level::High, Speed::Low); - // pwm.enable(Channel::Ch1); // VGA ground let _ = Output::new(pd5, Level::Low, Speed::Low); @@ -199,23 +132,10 @@ impl BrakingController { pub async fn arm_breaks(&mut self) { self.braking_rearm.set_low(); - self.data_sender - .send(Datapoint::new(Datatype::BrakingRearmDebug, 0, Instant::now().as_ticks())) - .await; - Timer::after_micros(10).await; + send_data!(self.data_sender, Datatype::BrakingRearmDebug, 0); + Timer::after_micros(50).await; // braking pcb only takes an instant to rearm the brakes self.braking_rearm.set_high(); - self.data_sender - .send(Datapoint::new(Datatype::BrakingRearmDebug, 1, Instant::now().as_ticks())) - .await; - - // let time_stamp = Instant::now(); - // while (Instant::now() - time_stamp) < Duration::from_millis(100) { - // if self.braking_communication.is_high() { - // self.brakes_extended = true; - // return true; - // } - // } - // false + send_data!(self.data_sender, Datatype::BrakingRearmDebug, 1); } #[allow(dead_code)] diff --git a/app/src/core/controllers/finite_state_machine_peripherals.rs b/app/src/core/controllers/finite_state_machine_peripherals.rs index 0846dbdaf..1bec19c7a 100644 --- a/app/src/core/controllers/finite_state_machine_peripherals.rs +++ b/app/src/core/controllers/finite_state_machine_peripherals.rs @@ -84,9 +84,6 @@ impl FSMPeripherals { ) .await; - // let mut b = Output::new(p.PB0, Level::High, Speed::High); - // b.set_high(); - debug!("creating can controller"); // the can controller configures both buses and then spawns all 4 read/write tasks. let can_controller = CanController::new( @@ -120,7 +117,18 @@ impl FSMPeripherals { .await; // the propulsion controller spawns tasks for reading current and voltage, and holds functions for setting the speed through the DAC - // let propulsion_controller = PropulsionController::new(); + let propulsion_controller = PropulsionController::new( + *x, + i.data_sender, + i.event_sender, + p.PA4, + p.DAC1, + p.ADC2, + p.PA5, + p.PA6, + p.PB1, + ) + .await; debug!("peripherals initialised."); // return this struct back to the FSM @@ -136,18 +144,7 @@ impl FSMPeripherals { dc_dc: Output::new(p.PD2, Level::Low, Speed::Low), }, red_led: Output::new(p.PB14, Level::Low, Speed::High), - propulsion_controller: PropulsionController::new( - *x, - i.data_sender, - i.event_sender, - p.PA4, - p.DAC1, - p.ADC2, - p.PA5, - p.PA6, - p.PB1, - ) - .await, + propulsion_controller, led_controller, } } diff --git a/app/src/core/controllers/hv_controller.rs b/app/src/core/controllers/hv_controller.rs index 6cf2f6db5..8238b22fa 100644 --- a/app/src/core/controllers/hv_controller.rs +++ b/app/src/core/controllers/hv_controller.rs @@ -27,8 +27,6 @@ impl HVPeripherals { #[cfg(debug_assertions)] info!("HV Powered on"); info!("HV Powered on"); - info!("HV Powered on"); - info!("HV Powered on"); } pub fn power_hv_off(&mut self) { diff --git a/app/src/core/controllers/propulsion_controller.rs b/app/src/core/controllers/propulsion_controller.rs index e6db20577..559316183 100644 --- a/app/src/core/controllers/propulsion_controller.rs +++ b/app/src/core/controllers/propulsion_controller.rs @@ -13,10 +13,9 @@ use embassy_stm32::peripherals::PA4; use embassy_stm32::peripherals::PA5; use embassy_stm32::peripherals::PA6; use embassy_stm32::peripherals::PB1; -use embassy_time::Instant; use embassy_time::Timer; -use crate::core::communication::Datapoint; +use crate::send_data; use crate::try_spawn; use crate::DataSender; use crate::Datatype; @@ -94,29 +93,15 @@ pub async fn read_prop_adc( mut pa5: PA5, mut pa6: PA6, ) { + Timer::after_millis(5000).await; loop { let v_ref_int = adc.read_internal(&mut v_ref_int_channel); let v = adc.read(&mut pa5) as u64; let i = adc.read(&mut pa6) as u64; - // #[cfg(debug_assertions)] - // defmt::info!( - // "Propulsion:\n\t voltage (pc0): {} \n\t current (pf3): {} \n\t reference: {}\n", - // v, i, v_ref_int - // ); - data_sender - .send(Datapoint::new(Datatype::PropulsionVoltage, v, Instant::now().as_ticks())) - .await; - data_sender - .send(Datapoint::new(Datatype::PropulsionCurrent, i, Instant::now().as_ticks())) - .await; - data_sender - .send(Datapoint::new( - Datatype::PropulsionVRefInt, - v_ref_int as u64, - Instant::now().as_ticks(), - )) - .await; + send_data!(data_sender, Datatype::PropulsionVoltage, v; 1000); + send_data!(data_sender, Datatype::PropulsionCurrent, i; 1000); + send_data!(data_sender, Datatype::PropulsionVRefInt, v_ref_int as u64; 1000); - Timer::after_millis(250).await; + Timer::after_millis(100).await; } } diff --git a/app/src/core/data/mod.rs b/app/src/core/data/mod.rs index 5f749bc69..89ff2e25c 100644 --- a/app/src/core/data/mod.rs +++ b/app/src/core/data/mod.rs @@ -5,13 +5,27 @@ mod batteries; mod sources; + +use embassy_time::Duration; +use embassy_time::Instant; +use heapless::Vec; + +use crate::pconfig::queue_event; +use crate::pconfig::ticks; +use crate::send_data; use crate::DataReceiver; use crate::DataSender; use crate::Datapoint; use crate::Datatype; use crate::Event; use crate::EventSender; +use crate::Info; use crate::ValueCheckResult; +use crate::HEARTBEATS; +use crate::HEARTBEATS_LEN; + +type HB = Vec<(Datatype, Duration, Option), { HEARTBEATS_LEN }>; + /// ## Individual handling of datapoints /// A lot of the subsystems on the pod use their own "encoding" for data. /// In order to make a reasonable matching between semantic meaning of @@ -22,37 +36,83 @@ pub async fn data_middle_step( outgoing: DataSender, event_sender: EventSender, ) -> ! { + let mut hb = HB::new(); + let hb_dt = HEARTBEATS.iter().map(|x| x.0).collect::>(); + loop { let data = incoming.receive().await; // 1. check thresholds match data.datatype.check_bounds(data.value) { ValueCheckResult::Fine => {}, - ValueCheckResult::Warn => { - outgoing.send(value_warning(data.datatype, data.value)).await; - }, + ValueCheckResult::Warn => send_data!( + outgoing, + Datatype::ValueWarning, + data.datatype.to_id() as u64, + data.value + ), ValueCheckResult::Error => { - outgoing.send(value_error(data.datatype, data.value)).await; + send_data!(outgoing, Datatype::ValueError, data.datatype.to_id() as u64, data.value) }, ValueCheckResult::BrakeNow => { - event_sender.send(Event::ValueOutOfBounds).await; - outgoing.send(value_critical(data.datatype, data.value)).await; + queue_event(event_sender, Event::ValueOutOfBounds).await; + send_data!( + outgoing, + Datatype::ValueCausedBraking, + data.datatype.to_id() as u64, + data.value + ); }, } - // 2. check specific data types + // 2. check heartbeats + let mut seen = !hb_dt.contains(&data.datatype); - outgoing.send(data).await; - } -} + for (dt, out, last) in hb.iter_mut() { + if !seen && *dt == data.datatype { + seen = true; + *last = Some(Instant::now()); + } else if last.is_some_and(|l| l.elapsed() > *out) { + event_sender.send(Event::EmergencyBraking).await; + outgoing + .send(Datapoint::new(Datatype::HeartbeatExpired, dt.to_id() as u64, ticks())) + .await; + *last = None; + } + } + if !seen { + match hb.push((data.datatype, timeout(data.datatype), None)) { + Ok(_) => {}, + Err(_) => { + send_data!(outgoing, Datatype::Info, Info::lamp_error_unreachable.to_idx()); + }, + } + } -fn value_warning(dt: Datatype, v: u64) -> Datapoint { - Datapoint::new(Datatype::ValueWarning, dt.to_id() as u64, v) -} + // 3. check for special cases -fn value_error(dt: Datatype, v: u64) -> Datapoint { - Datapoint::new(Datatype::ValueError, dt.to_id() as u64, v) + outgoing.send(data).await; + } } -fn value_critical(dt: Datatype, v: u64) -> Datapoint { - Datapoint::new(Datatype::ValueCausedBraking, dt.to_id() as u64, v) +fn timeout(dt: Datatype) -> Duration { + for (d, t) in HEARTBEATS { + if d == dt { + return Duration::from_millis(t); + } + } + Duration::from_millis(0) // This is unreachable, + // but as to not panic we return zero timeout. + // Since this will always be expired, it will always cause emergency braking } +// +// fn value_warning(dt: Datatype, v: u64) -> Datapoint { +// Datapoint::new(Datatype::ValueWarning, dt.to_id() as u64, v) +// } +// +// fn value_error(dt: Datatype, v: u64) -> Datapoint { +// Datapoint::new(Datatype::ValueError, dt.to_id() as u64, v) +// } +// +// fn value_critical(dt: Datatype, v: u64) -> Datapoint { +// Datapoint::new(Datatype::ValueCausedBraking, dt.to_id() as u64, v) +// } diff --git a/app/src/main.rs b/app/src/main.rs index ba47e3606..f22fb54c4 100644 --- a/app/src/main.rs +++ b/app/src/main.rs @@ -1,22 +1,8 @@ #![no_std] #![no_main] -#![allow( -// unused_must_use, -// unused_imports, -// unused_variables, -// unused_mut, -// dead_code, -// unreachable_code, -// unused_doc_comments, -// incomplete_features, - clippy::too_many_arguments -)] +#![allow(clippy::too_many_arguments)] #![deny(clippy::async_yields_async)] #![deny(rustdoc::broken_intra_doc_links)] -// #[warn(unused_must_use)] - -// Import absolutely EVERYTHING - use ::core::borrow::Borrow; use defmt::*; use defmt_rtt as _; @@ -61,13 +47,18 @@ bind_interrupts!(struct CanTwoInterrupts { FDCAN2_IT1 => can::IT1InterruptHandler; }); -/// Custom Data types----------------------- +// Custom Data types----------------------- + +/// A transmitter for the [`Datapoint`] MPMC [`DATA_QUEUE`] type DataSender = embassy_sync::channel::Sender<'static, NoopRawMutex, Datapoint, { DATA_QUEUE_SIZE }>; +/// A receiver for the [`Datapoint`] MPMC [`DATA_QUEUE`] type DataReceiver = embassy_sync::channel::Receiver<'static, NoopRawMutex, Datapoint, { DATA_QUEUE_SIZE }>; +/// A transmitter for the [`Event`] MPMC [`EVENT_QUEUE`] type EventSender = embassy_sync::priority_channel::Sender<'static, NoopRawMutex, Event, Max, { EVENT_QUEUE_SIZE }>; +/// A receiver for the [`Event`] MPMC [`EVENT_QUEUE`] type EventReceiver = embassy_sync::priority_channel::Receiver< 'static, NoopRawMutex, @@ -75,25 +66,33 @@ type EventReceiver = embassy_sync::priority_channel::Receiver< Max, { EVENT_QUEUE_SIZE }, >; +/// A transmitter for the [`can::frame::Frame`] MPMC [`CAN_ONE_QUEUE`]/[`CAN_TWO_QUEUE`] type CanSender = embassy_sync::channel::Sender<'static, NoopRawMutex, can::frame::Frame, { CAN_QUEUE_SIZE }>; +/// A receiver for the [`can::frame::Frame`] MPMC [`CAN_ONE_QUEUE`]/[`CAN_TWO_QUEUE`] type CanReceiver = embassy_sync::channel::Receiver<'static, NoopRawMutex, can::frame::Frame, { CAN_QUEUE_SIZE }>; -/// Static Allocations - MPMC queues +// Static Allocations - MPMC queues +/// The allocation for [`Event`]-[`embassy_sync::channel`] static EVENT_QUEUE: StaticCell> = StaticCell::new(); + +/// The allocation for [`Datapoint`]-[`embassy_sync::channel`] static DATA_QUEUE: StaticCell> = StaticCell::new(); - +/// The allocation for a [`Datapoint`]-[`embassy_sync::channel`] static PARSED_DATA_QUEUE: StaticCell> = StaticCell::new(); +/// The allocation for [`can::frame::Frame`]-[`embassy_sync::channel`] static CAN_ONE_QUEUE: StaticCell> = StaticCell::new(); +/// The allocation for [`can::frame::Frame`]-[`embassy_sync::channel`] static CAN_TWO_QUEUE: StaticCell> = StaticCell::new(); +/// Util struct for initialising [`FSMPeripherals`] pub struct InternalMessaging { event_sender: EventSender, data_sender: DataSender, diff --git a/app/src/pconfig.rs b/app/src/pconfig.rs index 46615c3d4..70bc9d111 100644 --- a/app/src/pconfig.rs +++ b/app/src/pconfig.rs @@ -1,3 +1,4 @@ +use defmt::error; use defmt::info; use embassy_net::IpAddress::Ipv4; use embassy_net::IpEndpoint; @@ -7,11 +8,18 @@ use embassy_stm32::rcc; use embassy_stm32::rcc::Pll; use embassy_stm32::rcc::*; use embassy_stm32::Config; +use embassy_time::Instant; use embedded_can::ExtendedId; use embedded_nal_async::Ipv4Addr; use embedded_nal_async::SocketAddr; use embedded_nal_async::SocketAddrV4; +use crate::core::communication::Datapoint; +use crate::DataSender; +use crate::Datatype; +use crate::Event; +use crate::EventSender; + #[inline] pub fn default_configuration() -> Config { let mut config = Config::default(); @@ -143,3 +151,113 @@ macro_rules! try_spawn { } }; } + +pub fn ticks() -> u64 { Instant::now().as_ticks() } + +/// Instantly sends an event through the MPMC queue. +/// * If the queue is full, the event will be discarded. +/// * This function will *not* block +pub fn send_event(event_sender: EventSender, event: Event) { + match event_sender.try_send(event) { + Ok(_) => {}, + Err(e) => { + error!("[send] event channel full: {:?}", e) + }, + } +} + +// /// Instantly sends a datapoint through the MPMC queue. +// /// * If the queue is full, the event will be discarded. +// /// * This function will *not* block. +// /// * The current timestamp will be used. +// /// * If a specific value for timestamp is needed, use [`send_dp`] instead. +// pub fn send_data(data_sender: DataSender, t: Datatype, data: u64) { +// match data_sender.try_send(Datapoint::new(t, data, ticks())) { +// Ok(_) => {} +// Err(e) => { +// error!("[send] data channel full: {:?}", e) +// } +// } +// } + +#[macro_export] +macro_rules! send_data { + ($data_sender:expr, $dtype:expr, $data:expr) => { + { + if let Err(e) = $data_sender.try_send( + $crate::Datapoint::new($dtype, $data, $crate::pconfig::ticks()) + ) { + defmt::error!("[send] data channel full: {:?}", e); + } + } + }; + ($data_sender:expr, $dtype:expr, $data:expr, $timestamp:expr) => { + { + if let Err(e) = $data_sender.try_send( + $crate::Datapoint::new($dtype, $data, $timestamp) + ) { + defmt::error!("[send] data channel full: {:?}", e); + } + } + }; + ($data_sender:expr, $dtype:expr, $data:expr; $timeout:expr) => { + { + if let Err(e) = $data_sender.try_send( + $crate::Datapoint::new($dtype, $data, $crate::pconfig::ticks()) + ) { + defmt::error!("[send] data channel full: {:?}", e); + embassy_time::Timer::after_millis($timeout).await; + } + } + }; + ($data_sender:expr, $dtype:expr, $data:expr, $timestamp:expr; $timeout:expr) => { + { + if let Err(e) = $data_sender.try_send( + $crate::core::communication::Datapoint::new($dtype, $data, $timestamp) + ) { + defmt::error!("[send] data channel full: {:?}", e); + embassy_time::Timer::after_millis($timeout).await; + } + } + }; +} + +// /// Instantly sends a datapoint through the MPMC queue. +// /// * If the queue is full, the event will be discarded. +// /// * This function will *not* block. +// /// * You need to specify a value for the timestamp (doesn't have to be a timestamp) +// /// * If you just want a timestamp as the timestamp, use [`send_data`] instead. +// pub fn send_dp(data_sender: DataSender, t: Datatype, d: u64, p: u64) { +// match data_sender.try_send(Datapoint::new(t, d, p)) { +// Ok(_) => {} +// Err(e) => { +// error!("[send] data channel full: {:?}", e) +// } +// } +// } + +/// Block the current task in order to send an event through the MPMC queue. +/// * If there's space in the event queue, this will complete instantly +/// * Otherwise it will `await` until there's space. +/// * *If the event isn't critical and the current task +/// shouldn't block, please use [`send_event`] instead* +pub async fn queue_event(event_sender: EventSender, event: Event) { event_sender.send(event).await } + +/// Block the current task in order to send a datapoint through the MPMC queue. +/// * If there's space in the event queue, this will complete instantly +/// * Otherwise it will `await` until there's space. +/// * *If the event isn't critical and the current task +/// shouldn't block, please use [`send_event`] instead* +pub async fn queue_data(data_sender: DataSender, t: Datatype, data: u64) { + data_sender.send(Datapoint::new(t, data, ticks())).await +} + +/// Block the current task in order to send a datapoint through the MPMC queue. +/// * If there's space in the event queue, this will complete instantly +/// * Otherwise it will `await` until there's space. +/// * *If the event isn't critical and the current task +/// shouldn't block, please use [`send_event`] instead* +/// * If you don't need to specify a specific timestamp, use [`queue_data`] instead +pub async fn queue_dp(data_sender: DataSender, t: Datatype, d: u64, p: u64) { + data_sender.send(Datapoint::new(t, d, p)).await +} diff --git a/config/commands.toml b/config/commands.toml index 40a2b9c19..19ac98af8 100644 --- a/config/commands.toml +++ b/config/commands.toml @@ -6,6 +6,10 @@ id = 0x01 name = "Heartbeat" id = 0x042 +[[Command]] +name = "FrontendHeartbeat" +id = 0x043 + # LEVITATION COMMANDS [[Command]] @@ -177,6 +181,10 @@ id = 0x65 name = "SetCurrentSpeed" id = 0x5a +[[Command]] +name = "CreateDatapoint" +id = 0x5b + [[Command]] name = "EmitEvent" id = 0x7A0 diff --git a/config/config.toml b/config/config.toml index f5cb222bc..ea5312c66 100644 --- a/config/config.toml +++ b/config/config.toml @@ -1,8 +1,7 @@ [gs] -ip = [192,168,1,3] +ip = [192,168,0,93] force = true port = 6942 -udp_port = 6943 buffer_size = 1460 # this is the MAXIMUM size of messages transmitted, in bytes. timeout = 1500 # this is the timeout for the socket, in milliseconds. heartbeat = 1000 # how often to send a keep_alive heartbeat, in milliseconds. @@ -14,22 +13,28 @@ shortcut_channel = "shortcut_channel" levi_exec_path = "C:/Users/kikoa/RustroverProjects/Helios_III/gs/station/Levi/windows-x86_64-debug/PmpGettingStartedCs.exe" [pod.net] -ip = [192, 168, 0, 199] -port = 17034 -udp_port = 17035 mac_addr = [0x00, 0x1e, 0x67, 0x4c, 0x5c, 0x3e] keep_alive = 1000 # keep alive interval, in milliseconds. [pod.internal] -event_queue_size = 128 -data_queue_size = 256 +event_queue_size = 256 +data_queue_size = 1024 can_queue_size = 128 -[pod.bms] -lv_ids = [0x19C, 0x19D, 0x19E, 0x19F, 0x1A0, 0x1A1, 0x1A2, 0x1A3, 0x1A4, 0x1A5, 0x1A6, 0x1BC, 0x1DC, 0x1FC, 0x29C,0x221] -hv_ids = [0x3A0, 0x3A1, 0x3A2, 0x3A3, 0x3A4, 0x3A5, 0x3A6, 0x3A7, 0x3A8, 0x3A9, 0x3AA, 0x3C0, 0x3E0, 0x400, 0x4A0, 0x425, 0x3C1, 0x3C2, 0x3C3, 0x3C4, 0x3C5, 0x3C6, 0x3C7, 0x3C8, 0x3C9, 0x3CA, 0x3CB, 0x3CC, 0x3CD,0x4A1, 0x4A3,0x4A4,0x4A5,0x4A6,0x4A7,0x4A8, 0x4A9,0x4AA,0x4AB,0x4AC,0x4AD] +[pod.comm] +bms_lv_ids = [0x19C, 0x19D, 0x19E, 0x19F, 0x1A0, 0x1A1, 0x1A2, 0x1A3, 0x1A4, 0x1A5, 0x1A6, 0x1BC, 0x1DC, 0x1FC, 0x29C,0x221] +bms_hv_ids = [0x3A0, 0x3A1, 0x3A2, 0x3A3, 0x3A4, 0x3A5, 0x3A6, 0x3A7, 0x3A8, 0x3A9, 0x3AA, 0x3C0, 0x3E0, 0x400, 0x4A0, 0x425, 0x3C1, 0x3C2, 0x3C3, 0x3C4, 0x3C5, 0x3C6, 0x3C7, 0x3C8, 0x3C9, 0x3CA, 0x3CB, 0x3CC, 0x3CD,0x4A1, 0x4A3,0x4A4,0x4A5,0x4A6,0x4A7,0x4A8, 0x4A9,0x4AA,0x4AB,0x4AC,0x4AD] gfd_ids = [0x37,0x38,0x39] sensor_hub = [0x1b,0x1c,0x1d,0x15] +levi_requested_data = ["Localisation", "PropulsionCurrent"] + +[pod.heartbeats] +# syntax: +# = +LocalisationHeartbeat = 1500 +FrontendHeartbeating = 1500 +SensorHubHeartbeat = 1500 + [[Info]] label = "ServerStarted" @@ -128,6 +133,10 @@ colour = "green" label = "EventsHashPassed" colour = "green" +[[Info]] +label = "ConfigHashPassed" +colour = "green" + [[Info]] label = "InvalidRouteConfigurationAbortingRun" colour = "red" @@ -140,3 +149,7 @@ colour = "green" label = "PodNotLevitating" colour = "red" +[[Info]] +label = "lamp_error_unreachable" +colour = "magenta" + diff --git a/config/datatypes.toml b/config/datatypes.toml index 0c8a16f78..f5bc92b40 100644 --- a/config/datatypes.toml +++ b/config/datatypes.toml @@ -847,6 +847,14 @@ id = 0x156 name = "levi_volt_avg" id = 0x157 +[[Datatype]] +name = "levi_location" +id = 0x117 + +[[Datatype]] +name = "levi_speed" +id = 0x116 + [[Datatype]] name = "BrakingCommDebug" id = 0x158 @@ -895,6 +903,10 @@ id = 0x168 name = "DataHash" id = 0x169 +[[Datatype]] +name = "ConfigHash" +id = 0x171 + [[Datatype]] name = "ValueError" id = 0x16a @@ -911,3 +923,19 @@ id = 0x16c name = "NextPositionDebug" id = 0x16d +[[Datatype]] +name = "LocalisationHeartbeat" +id = 0x16e + +[[Datatype]] +name = "SensorHubHeartbeat" +id = 0x16f + +[[Datatype]] +name = "FrontendHeartbeating" +id = 0x170 + +[[Datatype]] +name = "HeartbeatExpired" +id = 0x7fd + diff --git a/gs/docs/GDD.md b/gs/docs/GDD.md index c4247293b..a7f02c3cd 100644 --- a/gs/docs/GDD.md +++ b/gs/docs/GDD.md @@ -82,11 +82,12 @@ After registering a store to listen to and defining its type, one can later subs front-end! This is done using the `$storeName` syntax. For example, if you have a store named `BatteryBalanceLow`: ```sveltehtml +

{$lvBattery}

diff --git a/gs/docs/api/apiGDD.md b/gs/docs/api/apiGDD.md index bb021cf49..c992c0f3b 100644 --- a/gs/docs/api/apiGDD.md +++ b/gs/docs/api/apiGDD.md @@ -42,7 +42,7 @@ the stores can only have names of this type. This is ensured by the `NamedDataty |-------------------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------| | `registerStore(name: NamedDatatype, initial: T, dataConvFun?: (data: bigint, current: T) => T): void` | Register a store with the given name, initial value, and an optional process function. | | `updateStore(name: NamedDatatype, data: bigint)` | Update a store with the given name and data. | -| `getStore(name: NamedDatatype):Writable` | Get a store with the given name. (Which is any of `NamedDatatype` | +| `getWritable(name: NamedDatatype):Writable` | Get a store with the given name. (Which is any of `NamedDatatype` | ## Store diff --git a/gs/src/lib/components/BottomBar.svelte b/gs/src/lib/components/BottomBar.svelte index 4b75aa56e..64243045a 100644 --- a/gs/src/lib/components/BottomBar.svelte +++ b/gs/src/lib/components/BottomBar.svelte @@ -1,6 +1,7 @@ -