From e0ccd19b753b62d4f7c1983fd2f6424fdd23a9cd Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Sat, 29 Jan 2022 17:20:55 -0500 Subject: [PATCH 1/8] Create protobuf for outbound message --- avionics/src/main.rs | 239 +++++++++++++++++++++++++--------- avionics/src/messages.proto | 42 ++++++ avionics/src/protos/no_std.rs | 235 +++++++++++++++++++++++++++++++++ 3 files changed, 454 insertions(+), 62 deletions(-) diff --git a/avionics/src/main.rs b/avionics/src/main.rs index cc2ffbf..85313bb 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -24,8 +24,10 @@ use panic_semihosting as _; use rtic::cyccnt::{Instant, U32Ext as _}; pub mod protos; use protos::no_std::{ - mod_EpsResponse::OneOfresp, BatteryManagerState, BatteryManagerStates, BatteryVoltage, - BatteryVoltageState, CommandID, EpsCommand, EpsResponse, RailState, SolarVoltage, + mod_EpsResponse::OneOfresp, mod_RadioTelemetry, mod_RadioTelemetry::OneOfmessage, + BatteryManagerState, BatteryManagerStates, BatteryVoltage, BatteryVoltageState, CommandID, + EpsCommand, EpsResponse, PowerRails, RadioSOH, RadioTelemetry, RailSOH, RailState, + SolarVoltage, TelemetryID, }; use quick_protobuf::{deserialize_from_slice, serialize_into_slice, MessageWrite}; mod avi; @@ -161,71 +163,184 @@ const APP: () = { let vref = cx.resources.vref; let delay = cx.resources.delay; - loop { - // - // 1) - // Measure state & save it somewhere - //adc.calibrate(vref); - adc.set_sample_time(adc::SampleTime::Cycles47_5); - let th1 = adc.read(&mut analog_pins.th1).unwrap(); - let th2 = adc.read(&mut analog_pins.th2).unwrap(); - let th3 = adc.read(&mut analog_pins.th3).unwrap(); - let th4 = adc.read(&mut analog_pins.th4).unwrap(); - let th5 = adc.read(&mut analog_pins.th5).unwrap(); - let th6 = adc.read(&mut analog_pins.th6).unwrap(); - let th7 = adc.read(&mut analog_pins.th6).unwrap(); - let th8 = adc.read(&mut analog_pins.th6).unwrap(); - - // - // 2) - - // - // 3) - // Send any commands to the EPS - // Only send one at a time - if let Some(eps_command) = tx_queue.dequeue() { - let mut tmp_buf = [0u8; 1024]; - serialize_into_slice(&eps_command, &mut tmp_buf).ok(); - for elem in tmp_buf.iter().take(eps_command.get_size() + 1) { - block!(conn_tx.write(*elem)).unwrap(); + // Radio telemetry stuct sent to the radio + let mut radioTelemetry = RadioTelemetry { + tid: TelemetryID::SOH, + message: mod_RadioTelemetry::OneOfmessage::soh(RadioSOH { + batteryVoltage: Some(BatteryVoltage { + battery1: 0, + battery2: 0, + }), + solarVoltage: Some(SolarVoltage { + side1: 0, + side2: 0, + side3: 0, + side4: 0, + side5: 0, + side6: 0, + }), + batteryVoltageState: BatteryVoltageState::BothLow, + batteryManagerStates: Some(BatteryManagerStates { + battery1State: BatteryManagerState::Suspended, + battery2State: BatteryManagerState::Suspended, + }), + railSoh: Some(RailSOH { + rail1: false, + rail2: false, + rail3: false, + rail4: false, + rail5: false, + rail6: false, + rail7: false, + rail8: false, + rail9: false, + rail10: false, + rail11: false, + rail12: false, + rail13: false, + rail14: false, + rail15: false, + rail16: false, + hpwr1: false, + hpwr2: false, + hpwrEn: false, + }), + }), + }; + if let OneOfmessage::soh(ref mut radioSoh) = radioTelemetry.message { + loop { + // + // 1) + // Measure state & save it somewhere + //adc.calibrate(vref); + adc.set_sample_time(adc::SampleTime::Cycles47_5); + let th1 = adc.read(&mut analog_pins.th1).unwrap(); + let th2 = adc.read(&mut analog_pins.th2).unwrap(); + let th3 = adc.read(&mut analog_pins.th3).unwrap(); + let th4 = adc.read(&mut analog_pins.th4).unwrap(); + let th5 = adc.read(&mut analog_pins.th5).unwrap(); + let th6 = adc.read(&mut analog_pins.th6).unwrap(); + let th7 = adc.read(&mut analog_pins.th6).unwrap(); + let th8 = adc.read(&mut analog_pins.th6).unwrap(); + + // + // 2) + + // + // 3) + // Send any commands to the EPS + // Only send one at a time + if let Some(eps_command) = tx_queue.dequeue() { + let mut tmp_buf = [0u8; 1024]; + serialize_into_slice(&eps_command, &mut tmp_buf).ok(); + for elem in tmp_buf.iter().take(eps_command.get_size() + 1) { + block!(conn_tx.write(*elem)).unwrap(); + } } - } - - // - // 4) - // Loop over any responses we may have recieved and update state - while let Some(eps_response) = rx_queue.dequeue() { - match eps_response.cid { - CommandID::SetPowerRailState => {} - CommandID::GetPowerRailState => {} - CommandID::GetBatteryVoltage => {} - CommandID::GetSolarVoltage => {} - CommandID::GetBatteryVoltageState => {} - CommandID::GetBatteryManagerState => {} - }; - - let mut test_str_buffer = ArrayString::<512>::new(); - core::fmt::write( - &mut test_str_buffer, - format_args!("parsed message from EPS: {:?}\n\r", eps_response), - ) - .unwrap(); - // Write string buffer out to UART - for c in test_str_buffer.as_str().bytes() { - block!(debug_tx.write(c)).unwrap(); + // + // 4) + // Loop over any responses we may have recieved and update state + while let Some(eps_response) = rx_queue.dequeue() { + match eps_response.cid { + CommandID::SetPowerRailState => { /* Do nothing, this response is just an ACK*/ + } + // Update our internal storage with the rail state + CommandID::GetPowerRailState => match eps_response.resp { + OneOfresp::railState(ref rs) => { + if let Some(ref mut railSoh) = radioSoh.railSoh { + match rs.railIdx { + // This ought to be an index into an array, but it works fine as is. + PowerRails::Rail1 => railSoh.rail1 = rs.railState, + PowerRails::Rail2 => railSoh.rail2 = rs.railState, + PowerRails::Rail3 => railSoh.rail3 = rs.railState, + PowerRails::Rail4 => railSoh.rail4 = rs.railState, + PowerRails::Rail5 => railSoh.rail5 = rs.railState, + PowerRails::Rail6 => railSoh.rail6 = rs.railState, + PowerRails::Rail7 => railSoh.rail7 = rs.railState, + PowerRails::Rail8 => railSoh.rail8 = rs.railState, + PowerRails::Rail9 => railSoh.rail9 = rs.railState, + PowerRails::Rail10 => railSoh.rail10 = rs.railState, + PowerRails::Rail11 => railSoh.rail11 = rs.railState, + PowerRails::Rail12 => railSoh.rail12 = rs.railState, + PowerRails::Rail13 => railSoh.rail13 = rs.railState, + PowerRails::Rail14 => railSoh.rail14 = rs.railState, + PowerRails::Rail15 => railSoh.rail15 = rs.railState, + PowerRails::Rail16 => railSoh.rail16 = rs.railState, + PowerRails::Hpwr1 => railSoh.hpwr1 = rs.railState, + PowerRails::Hpwr2 => railSoh.hpwr2 = rs.railState, + PowerRails::HpwrEn => railSoh.hpwrEn = rs.railState, + } + } + } + OneOfresp::None => {} + _ => {} + }, + CommandID::GetBatteryVoltage => { + if let OneOfresp::batteryVoltage(ref bv) = eps_response.resp { + if let Some(ref mut batteryVoltage) = radioSoh.batteryVoltage { + batteryVoltage.battery1 = bv.battery1; + batteryVoltage.battery2 = bv.battery2; + } + } + } + CommandID::GetSolarVoltage => { + if let OneOfresp::solarVoltage(ref sv) = eps_response.resp { + if let Some(ref mut solarVoltage) = radioSoh.solarVoltage { + solarVoltage.side1 = sv.side1; + solarVoltage.side2 = sv.side2; + solarVoltage.side3 = sv.side3; + solarVoltage.side4 = sv.side4; + solarVoltage.side5 = sv.side5; + solarVoltage.side6 = sv.side6; + } + } + } + CommandID::GetBatteryVoltageState => { + if let OneOfresp::batteryVoltageState(ref bvs) = eps_response.resp { + radioSoh.batteryVoltageState = *bvs; + } + } + CommandID::GetBatteryManagerState => { + if let OneOfresp::batteryManagerStates(ref bms) = eps_response.resp { + if let Some(ref mut batteryManagerStates) = + radioSoh.batteryManagerStates + { + batteryManagerStates.battery1State = bms.battery1State; + batteryManagerStates.battery2State = bms.battery2State; + } + } + } + }; + + let mut test_str_buffer = ArrayString::<512>::new(); + core::fmt::write( + &mut test_str_buffer, + format_args!("parsed message from EPS: {:?}\n\r", eps_response), + ) + .unwrap(); + + // Write string buffer out to UART + for c in test_str_buffer.as_str().bytes() { + block!(debug_tx.write(c)).unwrap(); + } } - } - // - // 5) - // Pet watchdog - pet_watchdog(cx.resources.watchdog_done); + // + // 5) + // Pet watchdog + pet_watchdog(cx.resources.watchdog_done); - // - // 6) - // Sleep - delay.delay_ms(20u32); + // + // 6) + // Sleep + delay.delay_ms(20u32); + } + } + + // Should not return + loop { + cortex_m::asm::bkpt(); } } diff --git a/avionics/src/messages.proto b/avionics/src/messages.proto index f4732a6..1ac8547 100644 --- a/avionics/src/messages.proto +++ b/avionics/src/messages.proto @@ -86,3 +86,45 @@ message EpsResponse { BatteryManagerStates batteryManagerStates = 6; } } + + +enum TelemetryID { + SOH = 0; +} + +message RailSOH { + bool rail1 = 1; + bool rail2 = 2; + bool rail3 = 3; + bool rail4 = 4; + bool rail5 = 5; + bool rail6 = 6; + bool rail7 = 7; + bool rail8 = 8; + bool rail9 = 9; + bool rail10 = 10; + bool rail11 = 11; + bool rail12 = 12; + bool rail13 = 13; + bool rail14 = 14; + bool rail15 = 15; + bool rail16 = 16; + bool hpwr1 = 17; + bool hpwr2 = 18; + bool hpwrEn = 19; +} + +message RadioSOH { + BatteryVoltage batteryVoltage = 1; + SolarVoltage solarVoltage = 2; + BatteryVoltageState batteryVoltageState = 3; + BatteryManagerStates batteryManagerStates = 4; + RailSOH railSoh = 5; +} + +message RadioTelemetry { + TelemetryID tid = 1; + oneof message { + RadioSOH soh = 2; + } +} \ No newline at end of file diff --git a/avionics/src/protos/no_std.rs b/avionics/src/protos/no_std.rs index 0a752c0..3d74cd1 100644 --- a/avionics/src/protos/no_std.rs +++ b/avionics/src/protos/no_std.rs @@ -213,6 +213,35 @@ impl<'a> From<&'a str> for BatteryManagerState { } } +#[derive(Debug, PartialEq, Eq, Clone, Copy)] +pub enum TelemetryID { + SOH = 0, +} + +impl Default for TelemetryID { + fn default() -> Self { + TelemetryID::SOH + } +} + +impl From for TelemetryID { + fn from(i: i32) -> Self { + match i { + 0 => TelemetryID::SOH, + _ => Self::default(), + } + } +} + +impl<'a> From<&'a str> for TelemetryID { + fn from(s: &'a str) -> Self { + match s { + "SOH" => TelemetryID::SOH, + _ => Self::default(), + } + } +} + #[derive(Debug, Default, PartialEq, Clone)] pub struct RailState { pub railIdx: protos::no_std::PowerRails, @@ -476,3 +505,209 @@ impl Default for OneOfresp { } +#[derive(Debug, Default, PartialEq, Clone)] +pub struct RailSOH { + pub rail1: bool, + pub rail2: bool, + pub rail3: bool, + pub rail4: bool, + pub rail5: bool, + pub rail6: bool, + pub rail7: bool, + pub rail8: bool, + pub rail9: bool, + pub rail10: bool, + pub rail11: bool, + pub rail12: bool, + pub rail13: bool, + pub rail14: bool, + pub rail15: bool, + pub rail16: bool, + pub hpwr1: bool, + pub hpwr2: bool, + pub hpwrEn: bool, +} + +impl<'a> MessageRead<'a> for RailSOH { + fn from_reader(r: &mut BytesReader, bytes: &'a [u8]) -> Result { + let mut msg = Self::default(); + while !r.is_eof() { + match r.next_tag(bytes) { + Ok(8) => msg.rail1 = r.read_bool(bytes)?, + Ok(16) => msg.rail2 = r.read_bool(bytes)?, + Ok(24) => msg.rail3 = r.read_bool(bytes)?, + Ok(32) => msg.rail4 = r.read_bool(bytes)?, + Ok(40) => msg.rail5 = r.read_bool(bytes)?, + Ok(48) => msg.rail6 = r.read_bool(bytes)?, + Ok(56) => msg.rail7 = r.read_bool(bytes)?, + Ok(64) => msg.rail8 = r.read_bool(bytes)?, + Ok(72) => msg.rail9 = r.read_bool(bytes)?, + Ok(80) => msg.rail10 = r.read_bool(bytes)?, + Ok(88) => msg.rail11 = r.read_bool(bytes)?, + Ok(96) => msg.rail12 = r.read_bool(bytes)?, + Ok(104) => msg.rail13 = r.read_bool(bytes)?, + Ok(112) => msg.rail14 = r.read_bool(bytes)?, + Ok(120) => msg.rail15 = r.read_bool(bytes)?, + Ok(128) => msg.rail16 = r.read_bool(bytes)?, + Ok(136) => msg.hpwr1 = r.read_bool(bytes)?, + Ok(144) => msg.hpwr2 = r.read_bool(bytes)?, + Ok(152) => msg.hpwrEn = r.read_bool(bytes)?, + Ok(t) => { r.read_unknown(bytes, t)?; } + Err(e) => return Err(e), + } + } + Ok(msg) + } +} + +impl MessageWrite for RailSOH { + fn get_size(&self) -> usize { + 0 + + if self.rail1 == false { 0 } else { 1 + sizeof_varint(*(&self.rail1) as u64) } + + if self.rail2 == false { 0 } else { 1 + sizeof_varint(*(&self.rail2) as u64) } + + if self.rail3 == false { 0 } else { 1 + sizeof_varint(*(&self.rail3) as u64) } + + if self.rail4 == false { 0 } else { 1 + sizeof_varint(*(&self.rail4) as u64) } + + if self.rail5 == false { 0 } else { 1 + sizeof_varint(*(&self.rail5) as u64) } + + if self.rail6 == false { 0 } else { 1 + sizeof_varint(*(&self.rail6) as u64) } + + if self.rail7 == false { 0 } else { 1 + sizeof_varint(*(&self.rail7) as u64) } + + if self.rail8 == false { 0 } else { 1 + sizeof_varint(*(&self.rail8) as u64) } + + if self.rail9 == false { 0 } else { 1 + sizeof_varint(*(&self.rail9) as u64) } + + if self.rail10 == false { 0 } else { 1 + sizeof_varint(*(&self.rail10) as u64) } + + if self.rail11 == false { 0 } else { 1 + sizeof_varint(*(&self.rail11) as u64) } + + if self.rail12 == false { 0 } else { 1 + sizeof_varint(*(&self.rail12) as u64) } + + if self.rail13 == false { 0 } else { 1 + sizeof_varint(*(&self.rail13) as u64) } + + if self.rail14 == false { 0 } else { 1 + sizeof_varint(*(&self.rail14) as u64) } + + if self.rail15 == false { 0 } else { 1 + sizeof_varint(*(&self.rail15) as u64) } + + if self.rail16 == false { 0 } else { 2 + sizeof_varint(*(&self.rail16) as u64) } + + if self.hpwr1 == false { 0 } else { 2 + sizeof_varint(*(&self.hpwr1) as u64) } + + if self.hpwr2 == false { 0 } else { 2 + sizeof_varint(*(&self.hpwr2) as u64) } + + if self.hpwrEn == false { 0 } else { 2 + sizeof_varint(*(&self.hpwrEn) as u64) } + } + + fn write_message(&self, w: &mut Writer) -> Result<()> { + if self.rail1 != false { w.write_with_tag(8, |w| w.write_bool(*&self.rail1))?; } + if self.rail2 != false { w.write_with_tag(16, |w| w.write_bool(*&self.rail2))?; } + if self.rail3 != false { w.write_with_tag(24, |w| w.write_bool(*&self.rail3))?; } + if self.rail4 != false { w.write_with_tag(32, |w| w.write_bool(*&self.rail4))?; } + if self.rail5 != false { w.write_with_tag(40, |w| w.write_bool(*&self.rail5))?; } + if self.rail6 != false { w.write_with_tag(48, |w| w.write_bool(*&self.rail6))?; } + if self.rail7 != false { w.write_with_tag(56, |w| w.write_bool(*&self.rail7))?; } + if self.rail8 != false { w.write_with_tag(64, |w| w.write_bool(*&self.rail8))?; } + if self.rail9 != false { w.write_with_tag(72, |w| w.write_bool(*&self.rail9))?; } + if self.rail10 != false { w.write_with_tag(80, |w| w.write_bool(*&self.rail10))?; } + if self.rail11 != false { w.write_with_tag(88, |w| w.write_bool(*&self.rail11))?; } + if self.rail12 != false { w.write_with_tag(96, |w| w.write_bool(*&self.rail12))?; } + if self.rail13 != false { w.write_with_tag(104, |w| w.write_bool(*&self.rail13))?; } + if self.rail14 != false { w.write_with_tag(112, |w| w.write_bool(*&self.rail14))?; } + if self.rail15 != false { w.write_with_tag(120, |w| w.write_bool(*&self.rail15))?; } + if self.rail16 != false { w.write_with_tag(128, |w| w.write_bool(*&self.rail16))?; } + if self.hpwr1 != false { w.write_with_tag(136, |w| w.write_bool(*&self.hpwr1))?; } + if self.hpwr2 != false { w.write_with_tag(144, |w| w.write_bool(*&self.hpwr2))?; } + if self.hpwrEn != false { w.write_with_tag(152, |w| w.write_bool(*&self.hpwrEn))?; } + Ok(()) + } +} + +#[derive(Debug, Default, PartialEq, Clone)] +pub struct RadioSOH { + pub batteryVoltage: Option, + pub solarVoltage: Option, + pub batteryVoltageState: protos::no_std::BatteryVoltageState, + pub batteryManagerStates: Option, + pub railSoh: Option, +} + +impl<'a> MessageRead<'a> for RadioSOH { + fn from_reader(r: &mut BytesReader, bytes: &'a [u8]) -> Result { + let mut msg = Self::default(); + while !r.is_eof() { + match r.next_tag(bytes) { + Ok(10) => msg.batteryVoltage = Some(r.read_message::(bytes)?), + Ok(18) => msg.solarVoltage = Some(r.read_message::(bytes)?), + Ok(24) => msg.batteryVoltageState = r.read_enum(bytes)?, + Ok(34) => msg.batteryManagerStates = Some(r.read_message::(bytes)?), + Ok(42) => msg.railSoh = Some(r.read_message::(bytes)?), + Ok(t) => { r.read_unknown(bytes, t)?; } + Err(e) => return Err(e), + } + } + Ok(msg) + } +} + +impl MessageWrite for RadioSOH { + fn get_size(&self) -> usize { + 0 + + self.batteryVoltage.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + + self.solarVoltage.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + + if self.batteryVoltageState == protos::no_std::BatteryVoltageState::BothHigh { 0 } else { 1 + sizeof_varint(*(&self.batteryVoltageState) as u64) } + + self.batteryManagerStates.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + + self.railSoh.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + } + + fn write_message(&self, w: &mut Writer) -> Result<()> { + if let Some(ref s) = self.batteryVoltage { w.write_with_tag(10, |w| w.write_message(s))?; } + if let Some(ref s) = self.solarVoltage { w.write_with_tag(18, |w| w.write_message(s))?; } + if self.batteryVoltageState != protos::no_std::BatteryVoltageState::BothHigh { w.write_with_tag(24, |w| w.write_enum(*&self.batteryVoltageState as i32))?; } + if let Some(ref s) = self.batteryManagerStates { w.write_with_tag(34, |w| w.write_message(s))?; } + if let Some(ref s) = self.railSoh { w.write_with_tag(42, |w| w.write_message(s))?; } + Ok(()) + } +} + +#[derive(Debug, Default, PartialEq, Clone)] +pub struct RadioTelemetry { + pub tid: protos::no_std::TelemetryID, + pub message: protos::no_std::mod_RadioTelemetry::OneOfmessage, +} + +impl<'a> MessageRead<'a> for RadioTelemetry { + fn from_reader(r: &mut BytesReader, bytes: &'a [u8]) -> Result { + let mut msg = Self::default(); + while !r.is_eof() { + match r.next_tag(bytes) { + Ok(8) => msg.tid = r.read_enum(bytes)?, + Ok(18) => msg.message = protos::no_std::mod_RadioTelemetry::OneOfmessage::soh(r.read_message::(bytes)?), + Ok(t) => { r.read_unknown(bytes, t)?; } + Err(e) => return Err(e), + } + } + Ok(msg) + } +} + +impl MessageWrite for RadioTelemetry { + fn get_size(&self) -> usize { + 0 + + if self.tid == protos::no_std::TelemetryID::SOH { 0 } else { 1 + sizeof_varint(*(&self.tid) as u64) } + + match self.message { + protos::no_std::mod_RadioTelemetry::OneOfmessage::soh(ref m) => 1 + sizeof_len((m).get_size()), + protos::no_std::mod_RadioTelemetry::OneOfmessage::None => 0, + } } + + fn write_message(&self, w: &mut Writer) -> Result<()> { + if self.tid != protos::no_std::TelemetryID::SOH { w.write_with_tag(8, |w| w.write_enum(*&self.tid as i32))?; } + match self.message { protos::no_std::mod_RadioTelemetry::OneOfmessage::soh(ref m) => { w.write_with_tag(18, |w| w.write_message(m))? }, + protos::no_std::mod_RadioTelemetry::OneOfmessage::None => {}, + } Ok(()) + } +} + +pub mod mod_RadioTelemetry { + +use super::*; + +#[derive(Debug, PartialEq, Clone)] +pub enum OneOfmessage { + soh(protos::no_std::RadioSOH), + None, +} + +impl Default for OneOfmessage { + fn default() -> Self { + OneOfmessage::None + } +} + +} + From daac0ac64a9a4a998a1f216f461ebf33c9bf1895 Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Sat, 29 Jan 2022 23:58:11 -0500 Subject: [PATCH 2/8] Add USART3, USART3 tasks, & getPowerRail commands --- avionics/src/avi.rs | 30 +++++++++++- avionics/src/main.rs | 110 ++++++++++++++++++++++++++++++++++++++++--- 2 files changed, 133 insertions(+), 7 deletions(-) diff --git a/avionics/src/avi.rs b/avionics/src/avi.rs index a5a5b0c..6523ab9 100644 --- a/avionics/src/avi.rs +++ b/avionics/src/avi.rs @@ -1,5 +1,8 @@ extern crate stm32l4xx_hal as hal; -use hal::{adc, delay, gpio, prelude::*, serial, serial::Serial, stm32::UART4, stm32::USART2}; +use hal::{ + adc, delay, gpio, prelude::*, serial, serial::Serial, stm32::UART4, stm32::USART2, + stm32::USART3, +}; pub struct AVI { //device: hal::stm32::Peripherals, @@ -10,6 +13,8 @@ pub struct AVI { pub debug_tx: serial::Tx, pub conn_rx: serial::Rx, pub conn_tx: serial::Tx, + pub radio_rx: serial::Rx, + pub radio_tx: serial::Tx, pub led1: gpio::PF2>, pub led2: gpio::PF3>, pub led3: gpio::PF4>, @@ -211,6 +216,7 @@ impl AVI { // Create the tx & rx handles let (debug_tx, debug_rx) = debug_serial.split(); + // Setup the Serial abstraction for the EPS interface let conn_tx_pin = bank .gpioc .pc10 @@ -230,6 +236,26 @@ impl AVI { // Create the tx & rx handles let (conn_tx, conn_rx) = conn_serial.split(); + // Setup the serial abstraction for the radio interface + let radio_tx_pin = bank + .gpioc + .pc4 + .into_af7(&mut bank.gpioc.moder, &mut bank.gpioc.afrl); + let radio_rx_pin = bank + .gpioc + .pc5 + .into_af7(&mut bank.gpioc.moder, &mut bank.gpioc.afrl); + let mut radio_serial = Serial::usart3( + device.USART3, + (radio_tx_pin, radio_rx_pin), + serial::Config::default().baudrate(115_200.bps()), + clocks, + &mut rcc.apb1r1, + ); + radio_serial.listen(serial::Event::Rxne); + // Create the tx & rx handles + let (radio_tx, radio_rx) = radio_serial.split(); + AVI { //device, //parts: bank, @@ -239,6 +265,8 @@ impl AVI { debug_tx, conn_rx, conn_tx, + radio_rx, + radio_tx, led1, led2, led3, diff --git a/avionics/src/main.rs b/avionics/src/main.rs index 85313bb..e634cfe 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -17,7 +17,7 @@ use arrayvec::ArrayString; use core::alloc::Layout; use core::convert::Infallible; use cortex_m_semihosting as _; -use hal::{adc, delay, gpio, prelude::*, serial, stm32::UART4, stm32::USART2}; +use hal::{adc, delay, gpio, prelude::*, serial, stm32::UART4, stm32::USART2, stm32::USART3}; use heapless::{consts::*, spsc, Vec}; use nb::block; use panic_semihosting as _; @@ -46,10 +46,14 @@ const APP: () = { rx_cons: spsc::Consumer<'static, EpsResponse, U8>, tx_prod: spsc::Producer<'static, EpsCommand, U8>, tx_cons: spsc::Consumer<'static, EpsCommand, U8>, + radio_tx_prod: spsc::Producer<'static, RadioTelemetry, U8>, + radio_tx_cons: spsc::Consumer<'static, RadioTelemetry, U8>, debug_rx: serial::Rx, debug_tx: serial::Tx, conn_rx: serial::Rx, conn_tx: serial::Tx, + radio_rx: serial::Rx, + radio_tx: serial::Tx, led1: gpio::PF2>, led2: gpio::PF3>, led3: gpio::PF4>, @@ -62,14 +66,19 @@ const APP: () = { digital_pins: avi::DigitalPins, delay: delay::DelayCM, uart_parse_active: bool, + radio_uart_parse_active: bool, uart_parse_vec: &'static mut Vec, + radio_uart_parse_vec: &'static mut Vec, } #[init (schedule = [blinker, uart_buffer_clear, eps_query], spawn = [blinker])] fn init(cx: init::Context) -> init::LateResources { static mut RX_QUEUE: spsc::Queue = spsc::Queue(heapless::i::Queue::new()); static mut TX_QUEUE: spsc::Queue = spsc::Queue(heapless::i::Queue::new()); + static mut RADIO_TX_QUEUE: spsc::Queue = + spsc::Queue(heapless::i::Queue::new()); static mut UART_PARSE_VEC: Vec = Vec(heapless::i::Vec::new()); + static mut RADIO_UART_PARSE_VEC: Vec = Vec(heapless::i::Vec::new()); // Setup the Allocator // On the STM32L496 platform, there is 320K of RAM (shared by the stack) @@ -109,6 +118,9 @@ const APP: () = { // Create the producer and consumer sides of the Queue let (tx_prod, tx_cons) = TX_QUEUE.split(); + // Create the producer and consumer sides of the Queue + let (radio_tx_prod, radio_tx_cons) = RADIO_TX_QUEUE.split(); + // Schedule the blinking LED function cx.schedule .blinker(cx.start + BLINK_PERIOD.cycles()) @@ -124,20 +136,29 @@ const APP: () = { .uart_buffer_clear(cx.start + UART_PARSE_PERIOD.cycles()) .unwrap(); + // Schedule the RadioUart clear buffer function (only runs once at start) + cx.schedule + .uart_buffer_clear(cx.start + UART_PARSE_PERIOD.cycles()) + .unwrap(); + init::LateResources { rx_prod, rx_cons, tx_prod, tx_cons, + radio_tx_prod, + radio_tx_cons, debug_rx: avi.debug_rx, debug_tx: avi.debug_tx, conn_rx: avi.conn_rx, conn_tx: avi.conn_tx, - led1: avi.led1, - led2: avi.led2, - led3: avi.led3, - led4: avi.led4, - led5: avi.led5, + radio_rx: avi.radio_rx, + radio_tx: avi.radio_tx, + led1: avi.led1, // blinker + led2: avi.led2, // power + led3: avi.led3, // + led4: avi.led4, // radio uart buffer + led5: avi.led5, // eps uart buffer watchdog_done: avi.watchdog_done, vref: avi.vref, adc: avi.adc, @@ -145,7 +166,9 @@ const APP: () = { digital_pins: avi.digital_pins, delay: avi.delay, uart_parse_active: true, // true because we've scheduled the buffer clear function above + radio_uart_parse_active: true, // true because we've scheduled the buffer clear function above uart_parse_vec: UART_PARSE_VEC, + radio_uart_parse_vec: RADIO_UART_PARSE_VEC, } } @@ -390,6 +413,45 @@ const APP: () = { //} } + // This task and radio_uart_buffer_clear share the same priority, so they can't pre-empt each other + #[task(binds = USART3, resources = [led4, radio_rx, radio_uart_parse_active, radio_uart_parse_vec], schedule = [radio_uart_buffer_clear], priority = 3)] + fn usart3(cx: usart3::Context) { + let rx = cx.resources.radio_rx; + //let queue = cx.resources.radio_rx_prod; + let uart_parse_active = cx.resources.radio_uart_parse_active; + let uart_parse_vec = cx.resources.radio_uart_parse_vec; + if let Ok(b) = rx.read() { + // push byte onto vector queue + uart_parse_vec.push(b).ok(); + }; + + // TODO : do parsing? + + // Schedule the buffer clear activity (if it isn't already running) + if !(*uart_parse_active) { + *uart_parse_active = true; + cx.resources.led4.set_low().ok(); + cx.schedule + .radio_uart_buffer_clear(Instant::now() + UART_PARSE_PERIOD.cycles()) + .unwrap(); + } + } + // This task and usart3 share the same priority, so they can't pre-empt each other + #[task(resources = [led4, radio_uart_parse_active, radio_uart_parse_vec], priority = 3)] + fn radio_uart_buffer_clear(cx: radio_uart_buffer_clear::Context) { + let uart_parse_active = cx.resources.radio_uart_parse_active; + let uart_parse_vec = cx.resources.radio_uart_parse_vec; + let led4 = cx.resources.led4; + + // turn off LED + led4.set_high().ok(); + // Reset bool guard + *uart_parse_active = false; + + // Clear buffer + uart_parse_vec.clear(); + } + #[task(resources = [led1], schedule = [blinker], priority = 1)] fn blinker(cx: blinker::Context) { static mut LED_IS_ON: bool = false; @@ -413,9 +475,11 @@ const APP: () = { #[task(resources = [tx_prod], schedule = [eps_query], priority = 1)] fn eps_query(cx: eps_query::Context) { static mut CMD_TO_SEND: u8 = 0; + static mut RAIL_IDX: u8 = 0; let tx_prod = cx.resources.tx_prod; //cortex_m::asm::bkpt(); + // Enqueu one of these basic requests let cmd = match *CMD_TO_SEND { 1 => EpsCommand { cid: CommandID::GetSolarVoltage, @@ -435,6 +499,40 @@ const APP: () = { }, }; *CMD_TO_SEND = (*CMD_TO_SEND + 1) % 4; + tx_prod.enqueue(cmd).ok(); + + // Also enqueue a request to get a Power Rail State + let rail = match *RAIL_IDX { + 0 => PowerRails::Rail1, + 1 => PowerRails::Rail2, + 2 => PowerRails::Rail3, + 3 => PowerRails::Rail4, + 4 => PowerRails::Rail5, + 5 => PowerRails::Rail6, + 6 => PowerRails::Rail7, + 7 => PowerRails::Rail8, + 8 => PowerRails::Rail9, + 9 => PowerRails::Rail9, + 10 => PowerRails::Rail11, + 11 => PowerRails::Rail12, + 12 => PowerRails::Rail13, + 13 => PowerRails::Rail14, + 14 => PowerRails::Rail15, + 15 => PowerRails::Rail16, + 16 => PowerRails::Hpwr1, + 17 => PowerRails::Hpwr2, + 18 => PowerRails::HpwrEn, + _ => PowerRails::Rail1, + }; + + let cmd = EpsCommand { + cid: CommandID::GetPowerRailState, + railState: Some(RailState { + railIdx: rail, + railState: false, // this value will be ignored + }), + }; + *RAIL_IDX = (*RAIL_IDX + 1) % 19; tx_prod.enqueue(cmd).ok(); From 35fa85b73e241b4aef800b5decad7775e905cb91 Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Sun, 30 Jan 2022 14:22:52 -0500 Subject: [PATCH 3/8] In progress on radio communication state machine --- avionics/src/avi.rs | 18 ++++++ avionics/src/main.rs | 142 +++++++++++++++++++++++++++++++++++++++---- eps/src/main.rs | 19 ++++-- 3 files changed, 162 insertions(+), 17 deletions(-) diff --git a/avionics/src/avi.rs b/avionics/src/avi.rs index 6523ab9..c03e89c 100644 --- a/avionics/src/avi.rs +++ b/avionics/src/avi.rs @@ -4,6 +4,24 @@ use hal::{ stm32::USART3, }; +#[derive(core::fmt::Debug)] +pub enum RadioState { + RadioPowerFailure, + SendRadioOffCmd, + SendHpwrEnOffCmd, + RadioOff, + SendHpwrEnCmd, + WaitHpwrEnCmd1, + SendRadioOnCmd, + WaitRadioOnCmd1, + VerifyHpwrOnCmd, + WaitVerifyHpwrOnCmd1, + VerifyRadioOnCmd, + WaitVerifyRadioOnCmd1, + RadioOnNop, + RadioOnPopulateSOH, +} + pub struct AVI { //device: hal::stm32::Peripherals, //parts: GpioBankParts, diff --git a/avionics/src/main.rs b/avionics/src/main.rs index e634cfe..5901a23 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -31,6 +31,7 @@ use protos::no_std::{ }; use quick_protobuf::{deserialize_from_slice, serialize_into_slice, MessageWrite}; mod avi; +use avi::RadioState; #[global_allocator] static ALLOCATOR: CortexMHeap = CortexMHeap::empty(); @@ -186,8 +187,14 @@ const APP: () = { let vref = cx.resources.vref; let delay = cx.resources.delay; + let mut radioState = RadioState::RadioOff; + let mut receivedHpwrEnAck = false; + let mut receivedHpwr2Ack = false; + let mut receivedHpwrEnGetAck = false; + let mut receivedHpwr2GetAck = false; + // Radio telemetry stuct sent to the radio - let mut radioTelemetry = RadioTelemetry { + let mut telemetry = RadioTelemetry { tid: TelemetryID::SOH, message: mod_RadioTelemetry::OneOfmessage::soh(RadioSOH { batteryVoltage: Some(BatteryVoltage { @@ -230,7 +237,10 @@ const APP: () = { }), }), }; - if let OneOfmessage::soh(ref mut radioSoh) = radioTelemetry.message { + + // open a reference to radioSoh for convenience + if let OneOfmessage::soh(ref mut telemetry) = telemetry.message { + // Main loop, loop forever loop { // // 1) @@ -266,12 +276,18 @@ const APP: () = { // Loop over any responses we may have recieved and update state while let Some(eps_response) = rx_queue.dequeue() { match eps_response.cid { - CommandID::SetPowerRailState => { /* Do nothing, this response is just an ACK*/ - } + CommandID::SetPowerRailState => match eps_response.resp { + OneOfresp::railState(ref rs) => match rs.railIdx { + PowerRails::HpwrEn => receivedHpwrEnAck = true, + PowerRails::Hpwr2 => receivedHpwr2Ack = true, + _ => { /*Do nothing*/ } + }, + _ => { /*Do Nothing*/ } + }, // Update our internal storage with the rail state CommandID::GetPowerRailState => match eps_response.resp { OneOfresp::railState(ref rs) => { - if let Some(ref mut railSoh) = radioSoh.railSoh { + if let Some(ref mut railSoh) = telemetry.railSoh { match rs.railIdx { // This ought to be an index into an array, but it works fine as is. PowerRails::Rail1 => railSoh.rail1 = rs.railState, @@ -291,8 +307,14 @@ const APP: () = { PowerRails::Rail15 => railSoh.rail15 = rs.railState, PowerRails::Rail16 => railSoh.rail16 = rs.railState, PowerRails::Hpwr1 => railSoh.hpwr1 = rs.railState, - PowerRails::Hpwr2 => railSoh.hpwr2 = rs.railState, - PowerRails::HpwrEn => railSoh.hpwrEn = rs.railState, + PowerRails::Hpwr2 => { + railSoh.hpwr2 = rs.railState; + receivedHpwr2GetAck = true; + } + PowerRails::HpwrEn => { + railSoh.hpwrEn = rs.railState; + receivedHpwrEnGetAck = true; + } } } } @@ -301,7 +323,7 @@ const APP: () = { }, CommandID::GetBatteryVoltage => { if let OneOfresp::batteryVoltage(ref bv) = eps_response.resp { - if let Some(ref mut batteryVoltage) = radioSoh.batteryVoltage { + if let Some(ref mut batteryVoltage) = telemetry.batteryVoltage { batteryVoltage.battery1 = bv.battery1; batteryVoltage.battery2 = bv.battery2; } @@ -309,7 +331,7 @@ const APP: () = { } CommandID::GetSolarVoltage => { if let OneOfresp::solarVoltage(ref sv) = eps_response.resp { - if let Some(ref mut solarVoltage) = radioSoh.solarVoltage { + if let Some(ref mut solarVoltage) = telemetry.solarVoltage { solarVoltage.side1 = sv.side1; solarVoltage.side2 = sv.side2; solarVoltage.side3 = sv.side3; @@ -321,13 +343,13 @@ const APP: () = { } CommandID::GetBatteryVoltageState => { if let OneOfresp::batteryVoltageState(ref bvs) = eps_response.resp { - radioSoh.batteryVoltageState = *bvs; + telemetry.batteryVoltageState = *bvs; } } CommandID::GetBatteryManagerState => { if let OneOfresp::batteryManagerStates(ref bms) = eps_response.resp { if let Some(ref mut batteryManagerStates) = - radioSoh.batteryManagerStates + telemetry.batteryManagerStates { batteryManagerStates.battery1State = bms.battery1State; batteryManagerStates.battery2State = bms.battery2State; @@ -351,11 +373,31 @@ const APP: () = { // // 5) + // Calculate next radio state + let next_radio_state = run_radio_state_machine( + &radioState, + telemetry, + ( + receivedHpwrEnAck, + receivedHpwr2Ack, + receivedHpwrEnGetAck, + receivedHpwr2GetAck, + ), + ); + + // Reset these back to false + receivedHpwrEnAck = false; + receivedHpwr2Ack = false; + receivedHpwrEnGetAck = false; + receivedHpwr2GetAck = false; + + // + // 6) // Pet watchdog pet_watchdog(cx.resources.watchdog_done); // - // 6) + // 7) // Sleep delay.delay_ms(20u32); } @@ -550,6 +592,82 @@ const APP: () = { } }; +fn run_radio_state_machine( + current_radio_state: &RadioState, + soh: &RadioSOH, + recieved_ack: (bool, bool, bool, bool), +) -> RadioState { + match current_radio_state { + RadioState::RadioOff => match soh.batteryVoltageState { + BatteryVoltageState::BothHigh => RadioState::SendHpwrEnCmd, + _ => RadioState::RadioOff, + }, + RadioState::SendHpwrEnCmd => match recieved_ack { + (true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio + (false, _, _, _) => RadioState::WaitHpwrEnCmd1, // wait a little longer for response + }, + RadioState::WaitHpwrEnCmd1 => match recieved_ack { + (true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio + (false, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + RadioState::SendRadioOnCmd => match recieved_ack { + (_, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on + (_, false, _, _) => RadioState::WaitRadioOnCmd1, // wait a little longer for response + }, + RadioState::WaitRadioOnCmd1 => match recieved_ack { + (_, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on + (_, false, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + RadioState::VerifyHpwrOnCmd => match recieved_ack { + (_, _, true, _) => { + if soh.railSoh.as_ref().unwrap().hpwrEn { + RadioState::VerifyRadioOnCmd // Now verify if the radio rail is on + } else { + RadioState::WaitVerifyHpwrOnCmd1 // Wait a little longer + } + } + (_, _, false, _) => RadioState::WaitVerifyHpwrOnCmd1, // wait a little longer for response + }, + RadioState::WaitVerifyHpwrOnCmd1 => match recieved_ack { + (_, _, true, _) => { + if soh.railSoh.as_ref().unwrap().hpwrEn { + RadioState::VerifyRadioOnCmd // query explicitly to determine if radio power rail is on + } else { + RadioState::RadioPowerFailure // Bad, something failed + } + } + (_, _, false, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + RadioState::VerifyRadioOnCmd => match recieved_ack { + (_, _, _, true) => { + if soh.railSoh.as_ref().unwrap().hpwr2 { + RadioState::RadioOnNop // Radio is on + } else { + RadioState::WaitVerifyRadioOnCmd1 // wait a little longer + } + } + (_, _, _, false) => RadioState::WaitVerifyRadioOnCmd1, // wait a little longer for response + }, + RadioState::WaitVerifyRadioOnCmd1 => match recieved_ack { + (_, _, _, true) => { + if soh.railSoh.as_ref().unwrap().hpwr2 { + RadioState::RadioOnNop // Radio is on + } else { + RadioState::RadioPowerFailure // Bad, something failed + } + } + (_, _, _, false) => RadioState::RadioPowerFailure, // Bad, something failed + }, + RadioState::RadioOnNop => match soh.batteryVoltageState { + BatteryVoltageState::BothHigh => RadioState::RadioOnPopulateSOH, // Populate SOH if battery is still good + _ => RadioState::SendRadioOffCmd, // Turn off if battery is low + }, + RadioState::RadioOnPopulateSOH => RadioState::RadioOnNop, // loop back to RadioOnNop + + _ => RadioState::RadioOff, // TODO + } +} + //fn pet_watchdog(watchdog_done: &mut WatchdogPinType) { //fn pet_watchdog<'a>(watchdog_done: &'a mut hal::prelude::OutputPin) { fn pet_watchdog(pin: &mut impl OutputPin) { diff --git a/eps/src/main.rs b/eps/src/main.rs index 6596794..2b66a02 100644 --- a/eps/src/main.rs +++ b/eps/src/main.rs @@ -277,11 +277,20 @@ const APP: () = { rail_state[(railIdx as usize)] = railState; // Apply change to Power Rail apply_power_rail_state(&rail_state, digital_pins); - } - // return response - EpsResponse { - cid: eps_command.cid, - resp: mod_EpsResponse::OneOfresp::None, + // return response + EpsResponse { + cid: eps_command.cid, + resp: mod_EpsResponse::OneOfresp::railState(RailState { + railIdx: rs.railIdx, + railState: rs.railState, + }), + } + } else { + // return response + EpsResponse { + cid: eps_command.cid, + resp: mod_EpsResponse::OneOfresp::None, + } } } CommandID::GetPowerRailState => { From 0d6dad6c551a4dd59b07693b0a63276b01b4ccde Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Sun, 30 Jan 2022 23:31:07 -0500 Subject: [PATCH 4/8] Implement eps_cmd generation --- avionics/src/avi.rs | 4 +- avionics/src/main.rs | 138 +++++++++++++++++++++++++++++++++++++++---- 2 files changed, 130 insertions(+), 12 deletions(-) diff --git a/avionics/src/avi.rs b/avionics/src/avi.rs index c03e89c..b640596 100644 --- a/avionics/src/avi.rs +++ b/avionics/src/avi.rs @@ -4,11 +4,13 @@ use hal::{ stm32::USART3, }; -#[derive(core::fmt::Debug)] +#[derive(core::fmt::Debug, core::marker::Copy, core::clone::Clone)] pub enum RadioState { RadioPowerFailure, SendRadioOffCmd, + WaitRadioOffCmd, SendHpwrEnOffCmd, + WaitHpwrEnOffCmd, RadioOff, SendHpwrEnCmd, WaitHpwrEnCmd1, diff --git a/avionics/src/main.rs b/avionics/src/main.rs index 5901a23..745ade8 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -261,15 +261,6 @@ const APP: () = { // // 3) - // Send any commands to the EPS - // Only send one at a time - if let Some(eps_command) = tx_queue.dequeue() { - let mut tmp_buf = [0u8; 1024]; - serialize_into_slice(&eps_command, &mut tmp_buf).ok(); - for elem in tmp_buf.iter().take(eps_command.get_size() + 1) { - block!(conn_tx.write(*elem)).unwrap(); - } - } // // 4) @@ -385,6 +376,8 @@ const APP: () = { ), ); + let eps_cmd_rsm = apply_radio_state_machine(&mut radioState, &next_radio_state); + // Reset these back to false receivedHpwrEnAck = false; receivedHpwr2Ack = false; @@ -393,11 +386,21 @@ const APP: () = { // // 6) + // Send any commands to the EPS + // Only send one at a time + if let Some(eps_command) = eps_cmd_rsm { + send_eps_command(eps_command, conn_tx); + } else if let Some(eps_command) = tx_queue.dequeue() { + send_eps_command(eps_command, conn_tx); + } + + // + // 7) // Pet watchdog pet_watchdog(cx.resources.watchdog_done); // - // 7) + // 8) // Sleep delay.delay_ms(20u32); } @@ -592,16 +595,27 @@ const APP: () = { } }; +fn send_eps_command(eps_command: EpsCommand, conn_tx: &mut impl _embedded_hal_serial_Write) { + let mut tmp_buf = [0u8; 1024]; + serialize_into_slice(&eps_command, &mut tmp_buf).ok(); + for elem in tmp_buf.iter().take(eps_command.get_size() + 1) { + block!(conn_tx.write(*elem)).ok(); + } +} + fn run_radio_state_machine( current_radio_state: &RadioState, soh: &RadioSOH, recieved_ack: (bool, bool, bool, bool), ) -> RadioState { match current_radio_state { + // Radio off, determine if should go through power on RadioState::RadioOff => match soh.batteryVoltageState { BatteryVoltageState::BothHigh => RadioState::SendHpwrEnCmd, _ => RadioState::RadioOff, }, + + // Begin Radio Power on, Enable Hpwr RadioState::SendHpwrEnCmd => match recieved_ack { (true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio (false, _, _, _) => RadioState::WaitHpwrEnCmd1, // wait a little longer for response @@ -610,6 +624,8 @@ fn run_radio_state_machine( (true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio (false, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed }, + + // Turn on radio specifically (Hpwr2) RadioState::SendRadioOnCmd => match recieved_ack { (_, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on (_, false, _, _) => RadioState::WaitRadioOnCmd1, // wait a little longer for response @@ -618,6 +634,8 @@ fn run_radio_state_machine( (_, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on (_, false, _, _) => RadioState::RadioPowerFailure, // Bad, something failed }, + + // Verify HpwrEn is on RadioState::VerifyHpwrOnCmd => match recieved_ack { (_, _, true, _) => { if soh.railSoh.as_ref().unwrap().hpwrEn { @@ -638,6 +656,8 @@ fn run_radio_state_machine( } (_, _, false, _) => RadioState::RadioPowerFailure, // Bad, something failed }, + + // Verify Hpwr2 is on RadioState::VerifyRadioOnCmd => match recieved_ack { (_, _, _, true) => { if soh.railSoh.as_ref().unwrap().hpwr2 { @@ -658,13 +678,109 @@ fn run_radio_state_machine( } (_, _, _, false) => RadioState::RadioPowerFailure, // Bad, something failed }, + + // Radio On Steady State RadioState::RadioOnNop => match soh.batteryVoltageState { BatteryVoltageState::BothHigh => RadioState::RadioOnPopulateSOH, // Populate SOH if battery is still good _ => RadioState::SendRadioOffCmd, // Turn off if battery is low }, RadioState::RadioOnPopulateSOH => RadioState::RadioOnNop, // loop back to RadioOnNop - _ => RadioState::RadioOff, // TODO + // Transition to Radio Off. Turn of Hpwr2 + RadioState::SendRadioOffCmd => match recieved_ack { + (_, true, _, _) => RadioState::SendHpwrEnOffCmd, // transition to sending HpwrEnOff + (_, false, _, _) => RadioState::WaitRadioOffCmd, // wait a little longer for response + }, + RadioState::WaitRadioOffCmd => match recieved_ack { + (_, true, _, _) => RadioState::SendHpwrEnOffCmd, // transition to sending HpwrEnOff + (_, false, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + // Turn off HpwrEn + RadioState::SendHpwrEnOffCmd => match recieved_ack { + (true, _, _, _) => RadioState::RadioOff, // Radio is off + (false, _, _, _) => RadioState::WaitHpwrEnOffCmd, // wait a little longer for response + }, + RadioState::WaitHpwrEnOffCmd => match recieved_ack { + (true, _, _, _) => RadioState::RadioOff, // Radio is off + (false, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + + // TODO update this + RadioState::RadioPowerFailure => RadioState::RadioPowerFailure, + } +} + +fn apply_radio_state_machine( + current_radio_state: &mut RadioState, + next_radio_state: &RadioState, +) -> Option { + // Update current state with next state + *current_radio_state = *next_radio_state; + + match next_radio_state { + // Send Radio Off cmd + RadioState::SendRadioOffCmd => Some(EpsCommand { + cid: CommandID::SetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::Hpwr2, + railState: false, + }), + }), + RadioState::WaitRadioOffCmd => None, + + // Send HpwrEn off + RadioState::SendHpwrEnOffCmd => Some(EpsCommand { + cid: CommandID::SetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::HpwrEn, + railState: false, + }), + }), + RadioState::WaitHpwrEnOffCmd => None, + RadioState::RadioOff => None, + + // Send HpwrEn On cmd + RadioState::SendHpwrEnCmd => Some(EpsCommand { + cid: CommandID::SetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::HpwrEn, + railState: true, + }), + }), + RadioState::WaitHpwrEnCmd1 => None, + + // Send Radio On cmd + RadioState::SendRadioOnCmd => Some(EpsCommand { + cid: CommandID::SetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::Hpwr2, + railState: true, + }), + }), + RadioState::WaitRadioOnCmd1 => None, + + // Send Get HpwrEn state + RadioState::VerifyHpwrOnCmd => Some(EpsCommand { + cid: CommandID::GetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::HpwrEn, + railState: true, // ignored + }), + }), + RadioState::WaitVerifyHpwrOnCmd1 => None, + + // Send Get Hpwr2 state + RadioState::VerifyRadioOnCmd => Some(EpsCommand { + cid: CommandID::GetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::Hpwr2, + railState: true, // ignored + }), + }), + RadioState::WaitVerifyRadioOnCmd1 => None, + RadioState::RadioOnNop => None, + RadioState::RadioOnPopulateSOH => None, + RadioState::RadioPowerFailure => None, } } From 0e15e045ed05e58d6e5f51e659d7021a02870de0 Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Wed, 9 Mar 2022 23:10:41 -0500 Subject: [PATCH 5/8] Walking through state radio state machine works --- avionics/src/main.rs | 68 +++++++++++++++++++++++++++++++++++++++----- 1 file changed, 61 insertions(+), 7 deletions(-) diff --git a/avionics/src/main.rs b/avionics/src/main.rs index 745ade8..db149d0 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -37,7 +37,7 @@ use avi::RadioState; static ALLOCATOR: CortexMHeap = CortexMHeap::empty(); const BLINK_PERIOD: u32 = 8_000_000; -const EPS_QUERY_PERIOD: u32 = 3_000_000; +const EPS_QUERY_PERIOD: u32 = 4_000_000; const UART_PARSE_PERIOD: u32 = 1_000_000; #[rtic::app(device = hal::pac, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] @@ -72,7 +72,7 @@ const APP: () = { radio_uart_parse_vec: &'static mut Vec, } - #[init (schedule = [blinker, uart_buffer_clear, eps_query], spawn = [blinker])] + #[init (schedule = [blinker, uart_buffer_clear, radio_uart_buffer_clear, eps_query], spawn = [blinker])] fn init(cx: init::Context) -> init::LateResources { static mut RX_QUEUE: spsc::Queue = spsc::Queue(heapless::i::Queue::new()); static mut TX_QUEUE: spsc::Queue = spsc::Queue(heapless::i::Queue::new()); @@ -139,7 +139,7 @@ const APP: () = { // Schedule the RadioUart clear buffer function (only runs once at start) cx.schedule - .uart_buffer_clear(cx.start + UART_PARSE_PERIOD.cycles()) + .radio_uart_buffer_clear(cx.start + UART_PARSE_PERIOD.cycles()) .unwrap(); init::LateResources { @@ -352,7 +352,7 @@ const APP: () = { let mut test_str_buffer = ArrayString::<512>::new(); core::fmt::write( &mut test_str_buffer, - format_args!("parsed message from EPS: {:?}\n\r", eps_response), + format_args!("RX {:?}\n\r", eps_response), ) .unwrap(); @@ -376,6 +376,21 @@ const APP: () = { ), ); + // + // Log radio comms state + // + let mut test_str_buffer = ArrayString::<512>::new(); + core::fmt::write( + &mut test_str_buffer, + format_args!("({:?} <-- {:?}\n\r", next_radio_state, radioState), + ) + .unwrap(); + + // Write string buffer out to UART + for c in test_str_buffer.as_str().bytes() { + block!(debug_tx.write(c)).unwrap(); + } + let eps_cmd_rsm = apply_radio_state_machine(&mut radioState, &next_radio_state); // Reset these back to false @@ -384,25 +399,50 @@ const APP: () = { receivedHpwrEnGetAck = false; receivedHpwr2GetAck = false; + // + // Log command sent to EPS + // + //let mut test_str_buffer = ArrayString::<512>::new(); + // // 6) // Send any commands to the EPS // Only send one at a time if let Some(eps_command) = eps_cmd_rsm { + //core::fmt::write( + // &mut test_str_buffer, + // format_args!("To EPS {:?} \n\r", eps_command), + //) + //.unwrap(); send_eps_command(eps_command, conn_tx); } else if let Some(eps_command) = tx_queue.dequeue() { + //core::fmt::write( + // &mut test_str_buffer, + // format_args!("To EPS {:?} \n\r", eps_command), + //) + //.unwrap(); send_eps_command(eps_command, conn_tx); } + // Write string buffer out to UART + for c in test_str_buffer.as_str().bytes() { + block!(debug_tx.write(c)).unwrap(); + } + + // + // 7) Do Radio comms + // (If in correct state for radio messages, that is) + talk_to_radio(&radioState); + // - // 7) + // 8) // Pet watchdog pet_watchdog(cx.resources.watchdog_done); // - // 8) + // 9) // Sleep - delay.delay_ms(20u32); + delay.delay_ms(40u32); } } @@ -706,6 +746,7 @@ fn run_radio_state_machine( }, // TODO update this + // What do in this state RadioState::RadioPowerFailure => RadioState::RadioPowerFailure, } } @@ -784,6 +825,19 @@ fn apply_radio_state_machine( } } +fn talk_to_radio(current_radio_state: &RadioState) { + match current_radio_state { + RadioState::RadioOnPopulateSOH => { + // Do something... + //Send a message to the radio with the SOH information?? I think that is reasonable + } + _ => { /* Do Nothing */ } + } + //if *current_radio_state == RadioState::RadioOnPopulateSOH { + // // Do Something in here + //} +} + //fn pet_watchdog(watchdog_done: &mut WatchdogPinType) { //fn pet_watchdog<'a>(watchdog_done: &'a mut hal::prelude::OutputPin) { fn pet_watchdog(pin: &mut impl OutputPin) { From f61e0f06069e4c5933b8561101fe503c87219a8e Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Wed, 9 Mar 2022 23:42:06 -0500 Subject: [PATCH 6/8] Add 3.3V rail to state machine --- avionics/src/avi.rs | 6 ++ avionics/src/main.rs | 140 ++++++++++++++++++++++++++++++++++--------- 2 files changed, 117 insertions(+), 29 deletions(-) diff --git a/avionics/src/avi.rs b/avionics/src/avi.rs index b640596..a8629a1 100644 --- a/avionics/src/avi.rs +++ b/avionics/src/avi.rs @@ -11,7 +11,13 @@ pub enum RadioState { WaitRadioOffCmd, SendHpwrEnOffCmd, WaitHpwrEnOffCmd, + Send3V3RailOffCmd, + Wait3V3RailOffCmd, RadioOff, + Send3V3RailOnCmd, + Wait3V3RailOnCmd1, + Verify3V3RailOnCmd, + WaitVerify3V3RailOnCmd1, SendHpwrEnCmd, WaitHpwrEnCmd1, SendRadioOnCmd, diff --git a/avionics/src/main.rs b/avionics/src/main.rs index db149d0..ab717a3 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -188,6 +188,8 @@ const APP: () = { let delay = cx.resources.delay; let mut radioState = RadioState::RadioOff; + let mut received3V3RailAck = false; + let mut received3V3RailGetAck = false; let mut receivedHpwrEnAck = false; let mut receivedHpwr2Ack = false; let mut receivedHpwrEnGetAck = false; @@ -271,6 +273,7 @@ const APP: () = { OneOfresp::railState(ref rs) => match rs.railIdx { PowerRails::HpwrEn => receivedHpwrEnAck = true, PowerRails::Hpwr2 => receivedHpwr2Ack = true, + PowerRails::Rail10 => received3V3RailAck = true, _ => { /*Do nothing*/ } }, _ => { /*Do Nothing*/ } @@ -290,7 +293,10 @@ const APP: () = { PowerRails::Rail7 => railSoh.rail7 = rs.railState, PowerRails::Rail8 => railSoh.rail8 = rs.railState, PowerRails::Rail9 => railSoh.rail9 = rs.railState, - PowerRails::Rail10 => railSoh.rail10 = rs.railState, + PowerRails::Rail10 => { + railSoh.rail10 = rs.railState; + received3V3RailGetAck = true; + } PowerRails::Rail11 => railSoh.rail11 = rs.railState, PowerRails::Rail12 => railSoh.rail12 = rs.railState, PowerRails::Rail13 => railSoh.rail13 = rs.railState, @@ -369,6 +375,8 @@ const APP: () = { &radioState, telemetry, ( + received3V3RailAck, + received3V3RailGetAck, receivedHpwrEnAck, receivedHpwr2Ack, receivedHpwrEnGetAck, @@ -394,6 +402,8 @@ const APP: () = { let eps_cmd_rsm = apply_radio_state_machine(&mut radioState, &next_radio_state); // Reset these back to false + received3V3RailAck = false; + received3V3RailGetAck = false; receivedHpwrEnAck = false; receivedHpwr2Ack = false; receivedHpwrEnGetAck = false; @@ -646,77 +656,109 @@ fn send_eps_command(eps_command: EpsCommand, conn_tx: &mut impl _embedded_hal_se fn run_radio_state_machine( current_radio_state: &RadioState, soh: &RadioSOH, - recieved_ack: (bool, bool, bool, bool), + recieved_ack: (bool, bool, bool, bool, bool, bool), ) -> RadioState { match current_radio_state { // Radio off, determine if should go through power on RadioState::RadioOff => match soh.batteryVoltageState { - BatteryVoltageState::BothHigh => RadioState::SendHpwrEnCmd, + BatteryVoltageState::BothHigh => RadioState::Send3V3RailOnCmd, _ => RadioState::RadioOff, }, - // Begin Radio Power on, Enable Hpwr + // Begin Radio 3.3V rail power on + RadioState::Send3V3RailOnCmd => match recieved_ack { + (true, _, _, _, _, _) => RadioState::Verify3V3RailOnCmd, // query explicitly to determine if 3V3 rail is on + (false, _, _, _, _, _) => RadioState::Wait3V3RailOnCmd1, // wait a little longer for response + }, + RadioState::Wait3V3RailOnCmd1 => match recieved_ack { + (true, _, _, _, _, _) => RadioState::Verify3V3RailOnCmd, // query explicitly to determine if 3V3 rail is on + (false, _, _, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + + // Verify 3.3V is on + RadioState::Verify3V3RailOnCmd => match recieved_ack { + (_, true, _, _, _, _) => { + if soh.railSoh.as_ref().unwrap().rail10 { + RadioState::SendHpwrEnCmd // continue to power on the radio + } else { + RadioState::WaitVerify3V3RailOnCmd1 // Wait a little longer + } + } + (_, false, _, _, _, _) => RadioState::WaitVerify3V3RailOnCmd1, // wait a little longer for response + }, + RadioState::WaitVerify3V3RailOnCmd1 => match recieved_ack { + (_, true, _, _, _, _) => { + if soh.railSoh.as_ref().unwrap().rail10 { + RadioState::SendHpwrEnCmd // continue to power on the radio + } else { + RadioState::RadioPowerFailure // Bad, something failed + } + } + (_, false, _, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + + // Radio Hpwr Enable RadioState::SendHpwrEnCmd => match recieved_ack { - (true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio - (false, _, _, _) => RadioState::WaitHpwrEnCmd1, // wait a little longer for response + (_, _, true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio + (_, _, false, _, _, _) => RadioState::WaitHpwrEnCmd1, // wait a little longer for response }, RadioState::WaitHpwrEnCmd1 => match recieved_ack { - (true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio - (false, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + (_, _, true, _, _, _) => RadioState::SendRadioOnCmd, // continue to power on the radio + (_, _, false, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed }, // Turn on radio specifically (Hpwr2) RadioState::SendRadioOnCmd => match recieved_ack { - (_, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on - (_, false, _, _) => RadioState::WaitRadioOnCmd1, // wait a little longer for response + (_, _, _, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on + (_, _, _, false, _, _) => RadioState::WaitRadioOnCmd1, // wait a little longer for response }, RadioState::WaitRadioOnCmd1 => match recieved_ack { - (_, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on - (_, false, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + (_, _, _, true, _, _) => RadioState::VerifyHpwrOnCmd, // query explicitly to determine if radio power rail is on + (_, _, _, false, _, _) => RadioState::RadioPowerFailure, // Bad, something failed }, // Verify HpwrEn is on RadioState::VerifyHpwrOnCmd => match recieved_ack { - (_, _, true, _) => { + (_, _, _, _, true, _) => { if soh.railSoh.as_ref().unwrap().hpwrEn { RadioState::VerifyRadioOnCmd // Now verify if the radio rail is on } else { RadioState::WaitVerifyHpwrOnCmd1 // Wait a little longer } } - (_, _, false, _) => RadioState::WaitVerifyHpwrOnCmd1, // wait a little longer for response + (_, _, _, _, false, _) => RadioState::WaitVerifyHpwrOnCmd1, // wait a little longer for response }, RadioState::WaitVerifyHpwrOnCmd1 => match recieved_ack { - (_, _, true, _) => { + (_, _, _, _, true, _) => { if soh.railSoh.as_ref().unwrap().hpwrEn { RadioState::VerifyRadioOnCmd // query explicitly to determine if radio power rail is on } else { RadioState::RadioPowerFailure // Bad, something failed } } - (_, _, false, _) => RadioState::RadioPowerFailure, // Bad, something failed + (_, _, _, _, false, _) => RadioState::RadioPowerFailure, // Bad, something failed }, // Verify Hpwr2 is on RadioState::VerifyRadioOnCmd => match recieved_ack { - (_, _, _, true) => { + (_, _, _, _, _, true) => { if soh.railSoh.as_ref().unwrap().hpwr2 { RadioState::RadioOnNop // Radio is on } else { RadioState::WaitVerifyRadioOnCmd1 // wait a little longer } } - (_, _, _, false) => RadioState::WaitVerifyRadioOnCmd1, // wait a little longer for response + (_, _, _, _, _, false) => RadioState::WaitVerifyRadioOnCmd1, // wait a little longer for response }, RadioState::WaitVerifyRadioOnCmd1 => match recieved_ack { - (_, _, _, true) => { + (_, _, _, _, _, true) => { if soh.railSoh.as_ref().unwrap().hpwr2 { RadioState::RadioOnNop // Radio is on } else { RadioState::RadioPowerFailure // Bad, something failed } } - (_, _, _, false) => RadioState::RadioPowerFailure, // Bad, something failed + (_, _, _, _, _, false) => RadioState::RadioPowerFailure, // Bad, something failed }, // Radio On Steady State @@ -728,21 +770,31 @@ fn run_radio_state_machine( // Transition to Radio Off. Turn of Hpwr2 RadioState::SendRadioOffCmd => match recieved_ack { - (_, true, _, _) => RadioState::SendHpwrEnOffCmd, // transition to sending HpwrEnOff - (_, false, _, _) => RadioState::WaitRadioOffCmd, // wait a little longer for response + (_, _, _, true, _, _) => RadioState::SendHpwrEnOffCmd, // transition to sending HpwrEnOff + (_, _, _, false, _, _) => RadioState::WaitRadioOffCmd, // wait a little longer for response }, RadioState::WaitRadioOffCmd => match recieved_ack { - (_, true, _, _) => RadioState::SendHpwrEnOffCmd, // transition to sending HpwrEnOff - (_, false, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + (_, _, _, true, _, _) => RadioState::SendHpwrEnOffCmd, // transition to sending HpwrEnOff + (_, _, _, false, _, _) => RadioState::RadioPowerFailure, // Bad, something failed }, // Turn off HpwrEn RadioState::SendHpwrEnOffCmd => match recieved_ack { - (true, _, _, _) => RadioState::RadioOff, // Radio is off - (false, _, _, _) => RadioState::WaitHpwrEnOffCmd, // wait a little longer for response + (_, _, true, _, _, _) => RadioState::Send3V3RailOffCmd, // transition to sending 3V3 off + (_, _, false, _, _, _) => RadioState::WaitHpwrEnOffCmd, // wait a little longer for response }, RadioState::WaitHpwrEnOffCmd => match recieved_ack { - (true, _, _, _) => RadioState::RadioOff, // Radio is off - (false, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + (_, _, true, _, _, _) => RadioState::Send3V3RailOffCmd, // transition to sending 3V3 off + (_, _, false, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed + }, + + // Turn off 3V3 + RadioState::Send3V3RailOffCmd => match recieved_ack { + (true, _, _, _, _, _) => RadioState::RadioOff, // Radio is off + (false, _, _, _, _, _) => RadioState::Wait3V3RailOffCmd, // wait a little longer for response + }, + RadioState::Wait3V3RailOffCmd => match recieved_ack { + (true, _, _, _, _, _) => RadioState::RadioOff, // Radio is off + (false, _, _, _, _, _) => RadioState::RadioPowerFailure, // Bad, something failed }, // TODO update this @@ -759,7 +811,17 @@ fn apply_radio_state_machine( *current_radio_state = *next_radio_state; match next_radio_state { - // Send Radio Off cmd + // Send 3V3 Off cmd + RadioState::Send3V3RailOffCmd => Some(EpsCommand { + cid: CommandID::SetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::Rail10, + railState: false, + }), + }), + RadioState::Wait3V3RailOffCmd => None, + + // Send Hpwr2 Off cmd RadioState::SendRadioOffCmd => Some(EpsCommand { cid: CommandID::SetPowerRailState, railState: Some(RailState { @@ -780,6 +842,26 @@ fn apply_radio_state_machine( RadioState::WaitHpwrEnOffCmd => None, RadioState::RadioOff => None, + // Send 3V3 On cmd + RadioState::Send3V3RailOnCmd => Some(EpsCommand { + cid: CommandID::SetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::Rail10, + railState: true, + }), + }), + RadioState::Wait3V3RailOnCmd1 => None, + + // Send Get 3V3 state + RadioState::Verify3V3RailOnCmd => Some(EpsCommand { + cid: CommandID::GetPowerRailState, + railState: Some(RailState { + railIdx: PowerRails::Rail10, + railState: true, // ignored + }), + }), + RadioState::WaitVerify3V3RailOnCmd1 => None, + // Send HpwrEn On cmd RadioState::SendHpwrEnCmd => Some(EpsCommand { cid: CommandID::SetPowerRailState, From 20d6fbf0fb1da092e6dc7b7f57d9d41fa49aec22 Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Sun, 13 Mar 2022 22:30:56 -0400 Subject: [PATCH 7/8] Support sending commands to the radio --- avionics/Cargo.toml | 10 +- avionics/src/avi.rs | 18 ++- avionics/src/main.rs | 343 ++++++++++++++++++++++++++----------------- 3 files changed, 236 insertions(+), 135 deletions(-) diff --git a/avionics/Cargo.toml b/avionics/Cargo.toml index a1a9d14..b3fc3a7 100644 --- a/avionics/Cargo.toml +++ b/avionics/Cargo.toml @@ -12,10 +12,18 @@ cortex-m-semihosting = "0.3.3" cortex-m-rtic = "^0.5.5" # panic-halt = "0.2.0" panic-semihosting = "^0.5.6" -heapless = "^0.6.1" +heapless = "0.7.10" nb = "1.0.0" alloc-cortex-m = "0.4.1" +[dependencies.bitvec] +version = "1.0.0" +default-features = false + +[dependencies.packed_struct] +version = "0.10.0" +default-features = false + # Uncomment for the panic example. # panic-itm = "0.4.1" diff --git a/avionics/src/avi.rs b/avionics/src/avi.rs index a8629a1..1953c8f 100644 --- a/avionics/src/avi.rs +++ b/avionics/src/avi.rs @@ -1,4 +1,6 @@ extern crate stm32l4xx_hal as hal; +use packed_struct::prelude::*; + use hal::{ adc, delay, gpio, prelude::*, serial, serial::Serial, stm32::UART4, stm32::USART2, stm32::USART3, @@ -27,7 +29,21 @@ pub enum RadioState { VerifyRadioOnCmd, WaitVerifyRadioOnCmd1, RadioOnNop, - RadioOnPopulateSOH, + RadioOnAction, +} + +#[derive(core::fmt::Debug, core::marker::Copy, core::clone::Clone)] +pub enum RadioAction { + PopulateTelem, +} + +#[derive(PackedStruct, core::fmt::Debug, core::marker::Copy, core::clone::Clone)] +#[packed_struct(endian = "lsb")] +pub struct RadioMessageHeader { + pub hwid: u16, + pub sequence_number: u16, + pub destination: u8, + pub command_id: u8, } pub struct AVI { diff --git a/avionics/src/main.rs b/avionics/src/main.rs index ab717a3..d683393 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -15,11 +15,12 @@ extern crate stm32l4xx_hal as hal; use alloc_cortex_m::CortexMHeap; use arrayvec::ArrayString; use core::alloc::Layout; -use core::convert::Infallible; use cortex_m_semihosting as _; use hal::{adc, delay, gpio, prelude::*, serial, stm32::UART4, stm32::USART2, stm32::USART3}; -use heapless::{consts::*, spsc, Vec}; +use heapless::{spsc, Vec}; use nb::block; +use packed_struct::types::bits::ByteArray; +use packed_struct::PackedStruct; use panic_semihosting as _; use rtic::cyccnt::{Instant, U32Ext as _}; pub mod protos; @@ -31,6 +32,8 @@ use protos::no_std::{ }; use quick_protobuf::{deserialize_from_slice, serialize_into_slice, MessageWrite}; mod avi; +use avi::RadioAction; +use avi::RadioMessageHeader; use avi::RadioState; #[global_allocator] @@ -39,16 +42,17 @@ static ALLOCATOR: CortexMHeap = CortexMHeap::empty(); const BLINK_PERIOD: u32 = 8_000_000; const EPS_QUERY_PERIOD: u32 = 4_000_000; const UART_PARSE_PERIOD: u32 = 1_000_000; +const SEND_TELEM_TO_RADIO: u32 = 20; #[rtic::app(device = hal::pac, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] const APP: () = { struct Resources { - rx_prod: spsc::Producer<'static, EpsResponse, U8>, - rx_cons: spsc::Consumer<'static, EpsResponse, U8>, - tx_prod: spsc::Producer<'static, EpsCommand, U8>, - tx_cons: spsc::Consumer<'static, EpsCommand, U8>, - radio_tx_prod: spsc::Producer<'static, RadioTelemetry, U8>, - radio_tx_cons: spsc::Consumer<'static, RadioTelemetry, U8>, + rx_prod: spsc::Producer<'static, EpsResponse, 8>, + rx_cons: spsc::Consumer<'static, EpsResponse, 8>, + tx_prod: spsc::Producer<'static, EpsCommand, 8>, + tx_cons: spsc::Consumer<'static, EpsCommand, 8>, + radio_rx_prod: spsc::Producer<'static, RadioMessageHeader, 8>, + radio_rx_cons: spsc::Consumer<'static, RadioMessageHeader, 8>, debug_rx: serial::Rx, debug_tx: serial::Tx, conn_rx: serial::Rx, @@ -68,18 +72,17 @@ const APP: () = { delay: delay::DelayCM, uart_parse_active: bool, radio_uart_parse_active: bool, - uart_parse_vec: &'static mut Vec, - radio_uart_parse_vec: &'static mut Vec, + uart_parse_vec: &'static mut Vec, + radio_uart_parse_vec: &'static mut Vec, } #[init (schedule = [blinker, uart_buffer_clear, radio_uart_buffer_clear, eps_query], spawn = [blinker])] fn init(cx: init::Context) -> init::LateResources { - static mut RX_QUEUE: spsc::Queue = spsc::Queue(heapless::i::Queue::new()); - static mut TX_QUEUE: spsc::Queue = spsc::Queue(heapless::i::Queue::new()); - static mut RADIO_TX_QUEUE: spsc::Queue = - spsc::Queue(heapless::i::Queue::new()); - static mut UART_PARSE_VEC: Vec = Vec(heapless::i::Vec::new()); - static mut RADIO_UART_PARSE_VEC: Vec = Vec(heapless::i::Vec::new()); + static mut RX_QUEUE: spsc::Queue = spsc::Queue::new(); //spsc::Queue(heapless::i::Queue::new()); + static mut TX_QUEUE: spsc::Queue = spsc::Queue::new(); //spsc::Queue(heapless::i::Queue::new()); + static mut RADIO_RX_QUEUE: spsc::Queue = spsc::Queue::new(); + static mut UART_PARSE_VEC: Vec = Vec::new(); + static mut RADIO_UART_PARSE_VEC: Vec = Vec::new(); // Setup the Allocator // On the STM32L496 platform, there is 320K of RAM (shared by the stack) @@ -120,7 +123,7 @@ const APP: () = { let (tx_prod, tx_cons) = TX_QUEUE.split(); // Create the producer and consumer sides of the Queue - let (radio_tx_prod, radio_tx_cons) = RADIO_TX_QUEUE.split(); + let (radio_rx_prod, radio_rx_cons) = RADIO_RX_QUEUE.split(); // Schedule the blinking LED function cx.schedule @@ -147,8 +150,8 @@ const APP: () = { rx_cons, tx_prod, tx_cons, - radio_tx_prod, - radio_tx_cons, + radio_rx_prod, + radio_rx_cons, debug_rx: avi.debug_rx, debug_tx: avi.debug_tx, conn_rx: avi.conn_rx, @@ -157,7 +160,7 @@ const APP: () = { radio_tx: avi.radio_tx, led1: avi.led1, // blinker led2: avi.led2, // power - led3: avi.led3, // + led3: avi.led3, // sending to radio led4: avi.led4, // radio uart buffer led5: avi.led5, // eps uart buffer watchdog_done: avi.watchdog_done, @@ -173,20 +176,25 @@ const APP: () = { } } - #[idle(resources = [adc, vref, analog_pins, conn_tx, debug_tx, rx_cons, tx_cons, watchdog_done, digital_pins, led3, delay])] + #[idle(resources = [adc, vref, analog_pins, conn_tx, debug_tx, radio_tx, rx_cons, tx_cons, radio_rx_cons, watchdog_done, digital_pins, led3, delay])] fn idle(cx: idle::Context) -> ! { // Pull variables from the Context struct for conveinece. let rx_queue = cx.resources.rx_cons; let tx_queue = cx.resources.tx_cons; + let radio_rx_queue = cx.resources.radio_rx_cons; let debug_tx = cx.resources.debug_tx; let conn_tx = cx.resources.conn_tx; + let radio_tx = cx.resources.radio_tx; let adc = cx.resources.adc; let analog_pins = cx.resources.analog_pins; let _digital_pins = cx.resources.digital_pins; let led3 = cx.resources.led3; - let vref = cx.resources.vref; + let _vref = cx.resources.vref; let delay = cx.resources.delay; + let mut radio_nop_counts: u32 = 32; + let mut radio_message_sequence_number: u16 = 0; + let mut radioState = RadioState::RadioOff; let mut received3V3RailAck = false; let mut received3V3RailGetAck = false; @@ -196,7 +204,7 @@ const APP: () = { let mut receivedHpwr2GetAck = false; // Radio telemetry stuct sent to the radio - let mut telemetry = RadioTelemetry { + let mut radio_telemetry = RadioTelemetry { tid: TelemetryID::SOH, message: mod_RadioTelemetry::OneOfmessage::soh(RadioSOH { batteryVoltage: Some(BatteryVoltage { @@ -240,44 +248,59 @@ const APP: () = { }), }; - // open a reference to radioSoh for convenience - if let OneOfmessage::soh(ref mut telemetry) = telemetry.message { - // Main loop, loop forever - loop { + // Main loop, loop forever + loop { + // open a reference to radioSoh for convenience + if let OneOfmessage::soh(ref mut telemetry) = radio_telemetry.message { // // 1) // Measure state & save it somewhere //adc.calibrate(vref); adc.set_sample_time(adc::SampleTime::Cycles47_5); - let th1 = adc.read(&mut analog_pins.th1).unwrap(); - let th2 = adc.read(&mut analog_pins.th2).unwrap(); - let th3 = adc.read(&mut analog_pins.th3).unwrap(); - let th4 = adc.read(&mut analog_pins.th4).unwrap(); - let th5 = adc.read(&mut analog_pins.th5).unwrap(); - let th6 = adc.read(&mut analog_pins.th6).unwrap(); - let th7 = adc.read(&mut analog_pins.th6).unwrap(); - let th8 = adc.read(&mut analog_pins.th6).unwrap(); + let _th1 = adc.read(&mut analog_pins.th1).unwrap(); + let _th2 = adc.read(&mut analog_pins.th2).unwrap(); + let _th3 = adc.read(&mut analog_pins.th3).unwrap(); + let _th4 = adc.read(&mut analog_pins.th4).unwrap(); + let _th5 = adc.read(&mut analog_pins.th5).unwrap(); + let _th6 = adc.read(&mut analog_pins.th6).unwrap(); + let _th7 = adc.read(&mut analog_pins.th6).unwrap(); + let _th8 = adc.read(&mut analog_pins.th6).unwrap(); // // 2) // // 3) + // Loop over any responses we may have recieved from the radio + while let Some(radio_response) = radio_rx_queue.dequeue() { + // Simply log these out to console + let mut test_str_buffer = ArrayString::<256>::new(); + core::fmt::write( + &mut test_str_buffer, + format_args!("From Radio: {:?}\n\r", radio_response), + ) + .unwrap(); + // Write string buffer out to UART + for c in test_str_buffer.as_str().bytes() { + block!(debug_tx.write(c)).unwrap(); + } + } // // 4) - // Loop over any responses we may have recieved and update state + // Loop over any responses we may have recieved from the EPS and update state while let Some(eps_response) = rx_queue.dequeue() { match eps_response.cid { - CommandID::SetPowerRailState => match eps_response.resp { - OneOfresp::railState(ref rs) => match rs.railIdx { - PowerRails::HpwrEn => receivedHpwrEnAck = true, - PowerRails::Hpwr2 => receivedHpwr2Ack = true, - PowerRails::Rail10 => received3V3RailAck = true, - _ => { /*Do nothing*/ } - }, - _ => { /*Do Nothing*/ } - }, + CommandID::SetPowerRailState => { + if let OneOfresp::railState(ref rs) = eps_response.resp { + match rs.railIdx { + PowerRails::HpwrEn => receivedHpwrEnAck = true, + PowerRails::Hpwr2 => receivedHpwr2Ack = true, + PowerRails::Rail10 => received3V3RailAck = true, + _ => { /*Do nothing*/ } + } + } + } // Update our internal storage with the rail state CommandID::GetPowerRailState => match eps_response.resp { OneOfresp::railState(ref rs) => { @@ -367,11 +390,14 @@ const APP: () = { block!(debug_tx.write(c)).unwrap(); } } + } - // - // 5) - // Calculate next radio state - let next_radio_state = run_radio_state_machine( + // + // 5) + // Calculate next radio state + let mut next_radio_state: RadioState = RadioState::RadioPowerFailure; + if let OneOfmessage::soh(ref telemetry) = radio_telemetry.message { + next_radio_state = run_radio_state_machine( &radioState, telemetry, ( @@ -383,82 +409,92 @@ const APP: () = { receivedHpwr2GetAck, ), ); + } - // - // Log radio comms state - // - let mut test_str_buffer = ArrayString::<512>::new(); - core::fmt::write( - &mut test_str_buffer, - format_args!("({:?} <-- {:?}\n\r", next_radio_state, radioState), - ) - .unwrap(); - - // Write string buffer out to UART - for c in test_str_buffer.as_str().bytes() { - block!(debug_tx.write(c)).unwrap(); - } - - let eps_cmd_rsm = apply_radio_state_machine(&mut radioState, &next_radio_state); + // + // Log radio comms state + // + let mut test_str_buffer = ArrayString::<512>::new(); + core::fmt::write( + &mut test_str_buffer, + format_args!("({:?} <-- {:?}\n\r", next_radio_state, radioState), + ) + .unwrap(); - // Reset these back to false - received3V3RailAck = false; - received3V3RailGetAck = false; - receivedHpwrEnAck = false; - receivedHpwr2Ack = false; - receivedHpwrEnGetAck = false; - receivedHpwr2GetAck = false; + // Write string buffer out to UART + for c in test_str_buffer.as_str().bytes() { + block!(debug_tx.write(c)).unwrap(); + } - // - // Log command sent to EPS - // - //let mut test_str_buffer = ArrayString::<512>::new(); + let eps_cmd_rsm = apply_radio_state_machine(&mut radioState, &next_radio_state); + + // Reset these back to false + received3V3RailAck = false; + received3V3RailGetAck = false; + receivedHpwrEnAck = false; + receivedHpwr2Ack = false; + receivedHpwrEnGetAck = false; + receivedHpwr2GetAck = false; + + // + // Log command sent to EPS + // + //let mut test_str_buffer = ArrayString::<512>::new(); + + // + // 6) + // Send any commands to the EPS + // Only send one at a time + if let Some(eps_command) = eps_cmd_rsm { + //core::fmt::write( + // &mut test_str_buffer, + // format_args!("To EPS {:?} \n\r", eps_command), + //) + //.unwrap(); + send_eps_command(eps_command, conn_tx); + } else if let Some(eps_command) = tx_queue.dequeue() { + //core::fmt::write( + // &mut test_str_buffer, + // format_args!("To EPS {:?} \n\r", eps_command), + //) + //.unwrap(); + send_eps_command(eps_command, conn_tx); + } - // - // 6) - // Send any commands to the EPS - // Only send one at a time - if let Some(eps_command) = eps_cmd_rsm { - //core::fmt::write( - // &mut test_str_buffer, - // format_args!("To EPS {:?} \n\r", eps_command), - //) - //.unwrap(); - send_eps_command(eps_command, conn_tx); - } else if let Some(eps_command) = tx_queue.dequeue() { - //core::fmt::write( - // &mut test_str_buffer, - // format_args!("To EPS {:?} \n\r", eps_command), - //) - //.unwrap(); - send_eps_command(eps_command, conn_tx); - } + // Write string buffer out to UART + for c in test_str_buffer.as_str().bytes() { + block!(debug_tx.write(c)).unwrap(); + } - // Write string buffer out to UART - for c in test_str_buffer.as_str().bytes() { - block!(debug_tx.write(c)).unwrap(); + // + // 7) Figure out what to do with the radio (If the radio is in a good state ) + // (If in correct state for radio messages, that is) + match determine_radio_action(&radioState, &mut radio_nop_counts) { + Some(RadioAction::PopulateTelem) => { + let header = RadioMessageHeader { + hwid: 0xFFFF, // Indicates local message + sequence_number: radio_message_sequence_number, + destination: 1, // Don't forward this message + command_id: 0x1C, // Set external data 1 + }; + radio_message_sequence_number += 1; + led3.set_low().ok(); + send_radio_message(&header, &radio_telemetry, radio_tx); + led3.set_high().ok(); + // Do thing } - - // - // 7) Do Radio comms - // (If in correct state for radio messages, that is) - talk_to_radio(&radioState); - - // - // 8) - // Pet watchdog - pet_watchdog(cx.resources.watchdog_done); - - // - // 9) - // Sleep - delay.delay_ms(40u32); + None => { /* Do nothing */ } } - } - // Should not return - loop { - cortex_m::asm::bkpt(); + // + // 8) + // Pet watchdog + pet_watchdog(cx.resources.watchdog_done); + + // + // 9) + // Sleep + delay.delay_ms(40u32); } } @@ -509,10 +545,10 @@ const APP: () = { } // This task and radio_uart_buffer_clear share the same priority, so they can't pre-empt each other - #[task(binds = USART3, resources = [led4, radio_rx, radio_uart_parse_active, radio_uart_parse_vec], schedule = [radio_uart_buffer_clear], priority = 3)] + #[task(binds = USART3, resources = [led4, radio_rx, radio_rx_prod, radio_uart_parse_active, radio_uart_parse_vec], schedule = [radio_uart_buffer_clear], priority = 3)] fn usart3(cx: usart3::Context) { let rx = cx.resources.radio_rx; - //let queue = cx.resources.radio_rx_prod; + let queue = cx.resources.radio_rx_prod; let uart_parse_active = cx.resources.radio_uart_parse_active; let uart_parse_vec = cx.resources.radio_uart_parse_vec; if let Ok(b) = rx.read() { @@ -520,7 +556,20 @@ const APP: () = { uart_parse_vec.push(b).ok(); }; - // TODO : do parsing? + // If we have 6 bytes (the size of the header, parse) + if uart_parse_vec.len() == 6 { + // there has to be a better way + let array: [u8; 6] = [ + uart_parse_vec[0], + uart_parse_vec[1], + uart_parse_vec[2], + uart_parse_vec[3], + uart_parse_vec[4], + uart_parse_vec[5], + ]; + let header = RadioMessageHeader::unpack(&array).ok().unwrap(); + queue.enqueue(header).ok(); + } // Schedule the buffer clear activity (if it isn't already running) if !(*uart_parse_active) { @@ -646,13 +695,32 @@ const APP: () = { }; fn send_eps_command(eps_command: EpsCommand, conn_tx: &mut impl _embedded_hal_serial_Write) { - let mut tmp_buf = [0u8; 1024]; + let mut tmp_buf = [0u8; 512]; serialize_into_slice(&eps_command, &mut tmp_buf).ok(); for elem in tmp_buf.iter().take(eps_command.get_size() + 1) { block!(conn_tx.write(*elem)).ok(); } } +fn send_radio_message( + header: &RadioMessageHeader, + body: &RadioTelemetry, + conn_tx: &mut impl _embedded_hal_serial_Write, +) { + // write the header + let tmp_buf = header.pack().ok().unwrap(); + for elem in tmp_buf.as_bytes_slice().iter() { + block!(conn_tx.write(*elem)).ok(); + } + + // Write the body + let mut tmp_buf = [0u8; 128]; + serialize_into_slice(body, &mut tmp_buf).ok(); + for elem in tmp_buf.iter().take(body.get_size() + 1) { + block!(conn_tx.write(*elem)).ok(); + } +} + fn run_radio_state_machine( current_radio_state: &RadioState, soh: &RadioSOH, @@ -763,10 +831,10 @@ fn run_radio_state_machine( // Radio On Steady State RadioState::RadioOnNop => match soh.batteryVoltageState { - BatteryVoltageState::BothHigh => RadioState::RadioOnPopulateSOH, // Populate SOH if battery is still good + BatteryVoltageState::BothHigh => RadioState::RadioOnAction, // Keep Radio on if battery is still good _ => RadioState::SendRadioOffCmd, // Turn off if battery is low }, - RadioState::RadioOnPopulateSOH => RadioState::RadioOnNop, // loop back to RadioOnNop + RadioState::RadioOnAction => RadioState::RadioOnNop, // loop back to RadioOnNop // Transition to Radio Off. Turn of Hpwr2 RadioState::SendRadioOffCmd => match recieved_ack { @@ -902,22 +970,31 @@ fn apply_radio_state_machine( }), RadioState::WaitVerifyRadioOnCmd1 => None, RadioState::RadioOnNop => None, - RadioState::RadioOnPopulateSOH => None, + RadioState::RadioOnAction => None, RadioState::RadioPowerFailure => None, } } -fn talk_to_radio(current_radio_state: &RadioState) { +fn determine_radio_action( + current_radio_state: &RadioState, + radio_nop_counts: &mut u32, +) -> Option { match current_radio_state { - RadioState::RadioOnPopulateSOH => { - // Do something... - //Send a message to the radio with the SOH information?? I think that is reasonable + RadioState::RadioOnAction => { + if *radio_nop_counts % SEND_TELEM_TO_RADIO == 0 { + return Some(RadioAction::PopulateTelem); + } } - _ => { /* Do Nothing */ } - } - //if *current_radio_state == RadioState::RadioOnPopulateSOH { - // // Do Something in here - //} + RadioState::RadioOnNop => { + // Increment nop counts + *radio_nop_counts += 1; + } + _ => { + // Reset counter + *radio_nop_counts = 0; + } + }; + None } //fn pet_watchdog(watchdog_done: &mut WatchdogPinType) { From 43a1062c4d68118f147cd87489a9a06df1ca21fc Mon Sep 17 00:00:00 2001 From: Richard Wendel Date: Sun, 5 Jun 2022 13:44:20 -0400 Subject: [PATCH 8/8] Updated Protobuf --- avionics/src/main.rs | 52 +++--- avionics/src/messages.proto | 28 ++-- avionics/src/protos/no_std.rs | 52 +++--- eps/src/messages.proto | 68 ++++++-- eps/src/protos/no_std.rs | 287 +++++++++++++++++++++++++++++++--- 5 files changed, 388 insertions(+), 99 deletions(-) diff --git a/avionics/src/main.rs b/avionics/src/main.rs index d683393..9d3b4a9 100644 --- a/avionics/src/main.rs +++ b/avionics/src/main.rs @@ -42,7 +42,7 @@ static ALLOCATOR: CortexMHeap = CortexMHeap::empty(); const BLINK_PERIOD: u32 = 8_000_000; const EPS_QUERY_PERIOD: u32 = 4_000_000; const UART_PARSE_PERIOD: u32 = 1_000_000; -const SEND_TELEM_TO_RADIO: u32 = 20; +const SEND_TELEM_TO_RADIO: u32 = 5; #[rtic::app(device = hal::pac, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] const APP: () = { @@ -263,8 +263,8 @@ const APP: () = { let _th4 = adc.read(&mut analog_pins.th4).unwrap(); let _th5 = adc.read(&mut analog_pins.th5).unwrap(); let _th6 = adc.read(&mut analog_pins.th6).unwrap(); - let _th7 = adc.read(&mut analog_pins.th6).unwrap(); - let _th8 = adc.read(&mut analog_pins.th6).unwrap(); + let _th7 = adc.read(&mut analog_pins.th7).unwrap(); + let _th8 = adc.read(&mut analog_pins.th8).unwrap(); // // 2) @@ -462,13 +462,12 @@ const APP: () = { } // Write string buffer out to UART - for c in test_str_buffer.as_str().bytes() { - block!(debug_tx.write(c)).unwrap(); - } + //for c in test_str_buffer.as_str().bytes() { + // block!(debug_tx.write(c)).unwrap(); + //} // - // 7) Figure out what to do with the radio (If the radio is in a good state ) - // (If in correct state for radio messages, that is) + // 7) Figure out what to do with the radio (If in correct state for radio messages, that is) match determine_radio_action(&radioState, &mut radio_nop_counts) { Some(RadioAction::PopulateTelem) => { let header = RadioMessageHeader { @@ -556,16 +555,18 @@ const APP: () = { uart_parse_vec.push(b).ok(); }; - // If we have 6 bytes (the size of the header, parse) - if uart_parse_vec.len() == 6 { + // If we have 6 bytes (the size of the header, parse) + 3 of the preamble + // And the first two bytes line up with the expected preamble + if uart_parse_vec.len() == (6 + 3) && uart_parse_vec[0] == 0x22 && uart_parse_vec[1] == 0x69 + { // there has to be a better way let array: [u8; 6] = [ - uart_parse_vec[0], - uart_parse_vec[1], - uart_parse_vec[2], uart_parse_vec[3], uart_parse_vec[4], uart_parse_vec[5], + uart_parse_vec[6], + uart_parse_vec[7], + uart_parse_vec[8], ]; let header = RadioMessageHeader::unpack(&array).ok().unwrap(); queue.enqueue(header).ok(); @@ -656,7 +657,7 @@ const APP: () = { 6 => PowerRails::Rail7, 7 => PowerRails::Rail8, 8 => PowerRails::Rail9, - 9 => PowerRails::Rail9, + 9 => PowerRails::Rail10, 10 => PowerRails::Rail11, 11 => PowerRails::Rail12, 12 => PowerRails::Rail13, @@ -708,15 +709,26 @@ fn send_radio_message( conn_tx: &mut impl _embedded_hal_serial_Write, ) { // write the header - let tmp_buf = header.pack().ok().unwrap(); - for elem in tmp_buf.as_bytes_slice().iter() { + let header_buf = header.pack().ok().unwrap(); + let header_size = header_buf.len(); + + // Write the body + let mut body_buf = [0u8; 128]; + let body_size = body.get_size() + 1; + serialize_into_slice(body, &mut body_buf).ok(); + + let preamble: [u8; 3] = [0x22, 0x69, (header_size + body_size) as u8]; + + // These should be one for loop but I'm bad at rust + for elem in preamble.as_bytes_slice().iter() { block!(conn_tx.write(*elem)).ok(); } - // Write the body - let mut tmp_buf = [0u8; 128]; - serialize_into_slice(body, &mut tmp_buf).ok(); - for elem in tmp_buf.iter().take(body.get_size() + 1) { + for elem in header_buf.as_bytes_slice().iter() { + block!(conn_tx.write(*elem)).ok(); + } + + for elem in body_buf.iter().take(body_size) { block!(conn_tx.write(*elem)).ok(); } } diff --git a/avionics/src/messages.proto b/avionics/src/messages.proto index 1ac8547..a28e149 100644 --- a/avionics/src/messages.proto +++ b/avionics/src/messages.proto @@ -4,12 +4,12 @@ syntax = "proto3"; package protos.no_std; enum CommandID { - SetPowerRailState = 1; - GetPowerRailState = 2; - GetBatteryVoltage = 3; - GetSolarVoltage = 4; - GetBatteryVoltageState = 5; - GetBatteryManagerState = 6; + SetPowerRailState = 0; + GetPowerRailState = 1; + GetBatteryVoltage = 2; + GetSolarVoltage = 3; + GetBatteryVoltageState = 4; + GetBatteryManagerState = 5; } enum PowerRails { @@ -59,16 +59,16 @@ message SolarVoltage { } enum BatteryVoltageState { - BothHigh = 1; - B1HighB2Low = 2; - B1LowB2High = 3; - BothLow = 4; + BothHigh = 0; + B1HighB2Low = 1; + B1LowB2High = 2; + BothLow = 3; } enum BatteryManagerState { - Suspended = 1; - LowPower = 2; - HighPower = 3; + Suspended = 0; + LowPower = 1; + HighPower = 2; } message BatteryManagerStates { @@ -127,4 +127,4 @@ message RadioTelemetry { oneof message { RadioSOH soh = 2; } -} \ No newline at end of file +} diff --git a/avionics/src/protos/no_std.rs b/avionics/src/protos/no_std.rs index 3d74cd1..b0dcca1 100644 --- a/avionics/src/protos/no_std.rs +++ b/avionics/src/protos/no_std.rs @@ -15,12 +15,12 @@ use super::super::*; #[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum CommandID { - SetPowerRailState = 1, - GetPowerRailState = 2, - GetBatteryVoltage = 3, - GetSolarVoltage = 4, - GetBatteryVoltageState = 5, - GetBatteryManagerState = 6, + SetPowerRailState = 0, + GetPowerRailState = 1, + GetBatteryVoltage = 2, + GetSolarVoltage = 3, + GetBatteryVoltageState = 4, + GetBatteryManagerState = 5, } impl Default for CommandID { @@ -32,12 +32,12 @@ impl Default for CommandID { impl From for CommandID { fn from(i: i32) -> Self { match i { - 1 => CommandID::SetPowerRailState, - 2 => CommandID::GetPowerRailState, - 3 => CommandID::GetBatteryVoltage, - 4 => CommandID::GetSolarVoltage, - 5 => CommandID::GetBatteryVoltageState, - 6 => CommandID::GetBatteryManagerState, + 0 => CommandID::SetPowerRailState, + 1 => CommandID::GetPowerRailState, + 2 => CommandID::GetBatteryVoltage, + 3 => CommandID::GetSolarVoltage, + 4 => CommandID::GetBatteryVoltageState, + 5 => CommandID::GetBatteryManagerState, _ => Self::default(), } } @@ -142,10 +142,10 @@ impl<'a> From<&'a str> for PowerRails { #[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum BatteryVoltageState { - BothHigh = 1, - B1HighB2Low = 2, - B1LowB2High = 3, - BothLow = 4, + BothHigh = 0, + B1HighB2Low = 1, + B1LowB2High = 2, + BothLow = 3, } impl Default for BatteryVoltageState { @@ -157,10 +157,10 @@ impl Default for BatteryVoltageState { impl From for BatteryVoltageState { fn from(i: i32) -> Self { match i { - 1 => BatteryVoltageState::BothHigh, - 2 => BatteryVoltageState::B1HighB2Low, - 3 => BatteryVoltageState::B1LowB2High, - 4 => BatteryVoltageState::BothLow, + 0 => BatteryVoltageState::BothHigh, + 1 => BatteryVoltageState::B1HighB2Low, + 2 => BatteryVoltageState::B1LowB2High, + 3 => BatteryVoltageState::BothLow, _ => Self::default(), } } @@ -180,9 +180,9 @@ impl<'a> From<&'a str> for BatteryVoltageState { #[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum BatteryManagerState { - Suspended = 1, - LowPower = 2, - HighPower = 3, + Suspended = 0, + LowPower = 1, + HighPower = 2, } impl Default for BatteryManagerState { @@ -194,9 +194,9 @@ impl Default for BatteryManagerState { impl From for BatteryManagerState { fn from(i: i32) -> Self { match i { - 1 => BatteryManagerState::Suspended, - 2 => BatteryManagerState::LowPower, - 3 => BatteryManagerState::HighPower, + 0 => BatteryManagerState::Suspended, + 1 => BatteryManagerState::LowPower, + 2 => BatteryManagerState::HighPower, _ => Self::default(), } } diff --git a/eps/src/messages.proto b/eps/src/messages.proto index f4732a6..03bf972 100644 --- a/eps/src/messages.proto +++ b/eps/src/messages.proto @@ -4,12 +4,12 @@ syntax = "proto3"; package protos.no_std; enum CommandID { - SetPowerRailState = 1; - GetPowerRailState = 2; - GetBatteryVoltage = 3; - GetSolarVoltage = 4; - GetBatteryVoltageState = 5; - GetBatteryManagerState = 6; + SetPowerRailState = 0; + GetPowerRailState = 1; + GetBatteryVoltage = 2; + GetSolarVoltage = 3; + GetBatteryVoltageState = 4; + GetBatteryManagerState = 5; } enum PowerRails { @@ -59,16 +59,16 @@ message SolarVoltage { } enum BatteryVoltageState { - BothHigh = 1; - B1HighB2Low = 2; - B1LowB2High = 3; - BothLow = 4; + BothHigh = 0; + B1HighB2Low = 1; + B1LowB2High = 2; + BothLow = 3; } enum BatteryManagerState { - Suspended = 1; - LowPower = 2; - HighPower = 3; + Suspended = 0; + LowPower = 1; + HighPower = 2; } message BatteryManagerStates { @@ -86,3 +86,45 @@ message EpsResponse { BatteryManagerStates batteryManagerStates = 6; } } + + +enum TelemetryID { + SOH = 0; +} + +message RailSOH { + bool rail1 = 1; + bool rail2 = 2; + bool rail3 = 3; + bool rail4 = 4; + bool rail5 = 5; + bool rail6 = 6; + bool rail7 = 7; + bool rail8 = 8; + bool rail9 = 9; + bool rail10 = 10; + bool rail11 = 11; + bool rail12 = 12; + bool rail13 = 13; + bool rail14 = 14; + bool rail15 = 15; + bool rail16 = 16; + bool hpwr1 = 17; + bool hpwr2 = 18; + bool hpwrEn = 19; +} + +message RadioSOH { + BatteryVoltage batteryVoltage = 1; + SolarVoltage solarVoltage = 2; + BatteryVoltageState batteryVoltageState = 3; + BatteryManagerStates batteryManagerStates = 4; + RailSOH railSoh = 5; +} + +message RadioTelemetry { + TelemetryID tid = 1; + oneof message { + RadioSOH soh = 2; + } +} \ No newline at end of file diff --git a/eps/src/protos/no_std.rs b/eps/src/protos/no_std.rs index 0a752c0..b0dcca1 100644 --- a/eps/src/protos/no_std.rs +++ b/eps/src/protos/no_std.rs @@ -15,12 +15,12 @@ use super::super::*; #[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum CommandID { - SetPowerRailState = 1, - GetPowerRailState = 2, - GetBatteryVoltage = 3, - GetSolarVoltage = 4, - GetBatteryVoltageState = 5, - GetBatteryManagerState = 6, + SetPowerRailState = 0, + GetPowerRailState = 1, + GetBatteryVoltage = 2, + GetSolarVoltage = 3, + GetBatteryVoltageState = 4, + GetBatteryManagerState = 5, } impl Default for CommandID { @@ -32,12 +32,12 @@ impl Default for CommandID { impl From for CommandID { fn from(i: i32) -> Self { match i { - 1 => CommandID::SetPowerRailState, - 2 => CommandID::GetPowerRailState, - 3 => CommandID::GetBatteryVoltage, - 4 => CommandID::GetSolarVoltage, - 5 => CommandID::GetBatteryVoltageState, - 6 => CommandID::GetBatteryManagerState, + 0 => CommandID::SetPowerRailState, + 1 => CommandID::GetPowerRailState, + 2 => CommandID::GetBatteryVoltage, + 3 => CommandID::GetSolarVoltage, + 4 => CommandID::GetBatteryVoltageState, + 5 => CommandID::GetBatteryManagerState, _ => Self::default(), } } @@ -142,10 +142,10 @@ impl<'a> From<&'a str> for PowerRails { #[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum BatteryVoltageState { - BothHigh = 1, - B1HighB2Low = 2, - B1LowB2High = 3, - BothLow = 4, + BothHigh = 0, + B1HighB2Low = 1, + B1LowB2High = 2, + BothLow = 3, } impl Default for BatteryVoltageState { @@ -157,10 +157,10 @@ impl Default for BatteryVoltageState { impl From for BatteryVoltageState { fn from(i: i32) -> Self { match i { - 1 => BatteryVoltageState::BothHigh, - 2 => BatteryVoltageState::B1HighB2Low, - 3 => BatteryVoltageState::B1LowB2High, - 4 => BatteryVoltageState::BothLow, + 0 => BatteryVoltageState::BothHigh, + 1 => BatteryVoltageState::B1HighB2Low, + 2 => BatteryVoltageState::B1LowB2High, + 3 => BatteryVoltageState::BothLow, _ => Self::default(), } } @@ -180,9 +180,9 @@ impl<'a> From<&'a str> for BatteryVoltageState { #[derive(Debug, PartialEq, Eq, Clone, Copy)] pub enum BatteryManagerState { - Suspended = 1, - LowPower = 2, - HighPower = 3, + Suspended = 0, + LowPower = 1, + HighPower = 2, } impl Default for BatteryManagerState { @@ -194,9 +194,9 @@ impl Default for BatteryManagerState { impl From for BatteryManagerState { fn from(i: i32) -> Self { match i { - 1 => BatteryManagerState::Suspended, - 2 => BatteryManagerState::LowPower, - 3 => BatteryManagerState::HighPower, + 0 => BatteryManagerState::Suspended, + 1 => BatteryManagerState::LowPower, + 2 => BatteryManagerState::HighPower, _ => Self::default(), } } @@ -213,6 +213,35 @@ impl<'a> From<&'a str> for BatteryManagerState { } } +#[derive(Debug, PartialEq, Eq, Clone, Copy)] +pub enum TelemetryID { + SOH = 0, +} + +impl Default for TelemetryID { + fn default() -> Self { + TelemetryID::SOH + } +} + +impl From for TelemetryID { + fn from(i: i32) -> Self { + match i { + 0 => TelemetryID::SOH, + _ => Self::default(), + } + } +} + +impl<'a> From<&'a str> for TelemetryID { + fn from(s: &'a str) -> Self { + match s { + "SOH" => TelemetryID::SOH, + _ => Self::default(), + } + } +} + #[derive(Debug, Default, PartialEq, Clone)] pub struct RailState { pub railIdx: protos::no_std::PowerRails, @@ -476,3 +505,209 @@ impl Default for OneOfresp { } +#[derive(Debug, Default, PartialEq, Clone)] +pub struct RailSOH { + pub rail1: bool, + pub rail2: bool, + pub rail3: bool, + pub rail4: bool, + pub rail5: bool, + pub rail6: bool, + pub rail7: bool, + pub rail8: bool, + pub rail9: bool, + pub rail10: bool, + pub rail11: bool, + pub rail12: bool, + pub rail13: bool, + pub rail14: bool, + pub rail15: bool, + pub rail16: bool, + pub hpwr1: bool, + pub hpwr2: bool, + pub hpwrEn: bool, +} + +impl<'a> MessageRead<'a> for RailSOH { + fn from_reader(r: &mut BytesReader, bytes: &'a [u8]) -> Result { + let mut msg = Self::default(); + while !r.is_eof() { + match r.next_tag(bytes) { + Ok(8) => msg.rail1 = r.read_bool(bytes)?, + Ok(16) => msg.rail2 = r.read_bool(bytes)?, + Ok(24) => msg.rail3 = r.read_bool(bytes)?, + Ok(32) => msg.rail4 = r.read_bool(bytes)?, + Ok(40) => msg.rail5 = r.read_bool(bytes)?, + Ok(48) => msg.rail6 = r.read_bool(bytes)?, + Ok(56) => msg.rail7 = r.read_bool(bytes)?, + Ok(64) => msg.rail8 = r.read_bool(bytes)?, + Ok(72) => msg.rail9 = r.read_bool(bytes)?, + Ok(80) => msg.rail10 = r.read_bool(bytes)?, + Ok(88) => msg.rail11 = r.read_bool(bytes)?, + Ok(96) => msg.rail12 = r.read_bool(bytes)?, + Ok(104) => msg.rail13 = r.read_bool(bytes)?, + Ok(112) => msg.rail14 = r.read_bool(bytes)?, + Ok(120) => msg.rail15 = r.read_bool(bytes)?, + Ok(128) => msg.rail16 = r.read_bool(bytes)?, + Ok(136) => msg.hpwr1 = r.read_bool(bytes)?, + Ok(144) => msg.hpwr2 = r.read_bool(bytes)?, + Ok(152) => msg.hpwrEn = r.read_bool(bytes)?, + Ok(t) => { r.read_unknown(bytes, t)?; } + Err(e) => return Err(e), + } + } + Ok(msg) + } +} + +impl MessageWrite for RailSOH { + fn get_size(&self) -> usize { + 0 + + if self.rail1 == false { 0 } else { 1 + sizeof_varint(*(&self.rail1) as u64) } + + if self.rail2 == false { 0 } else { 1 + sizeof_varint(*(&self.rail2) as u64) } + + if self.rail3 == false { 0 } else { 1 + sizeof_varint(*(&self.rail3) as u64) } + + if self.rail4 == false { 0 } else { 1 + sizeof_varint(*(&self.rail4) as u64) } + + if self.rail5 == false { 0 } else { 1 + sizeof_varint(*(&self.rail5) as u64) } + + if self.rail6 == false { 0 } else { 1 + sizeof_varint(*(&self.rail6) as u64) } + + if self.rail7 == false { 0 } else { 1 + sizeof_varint(*(&self.rail7) as u64) } + + if self.rail8 == false { 0 } else { 1 + sizeof_varint(*(&self.rail8) as u64) } + + if self.rail9 == false { 0 } else { 1 + sizeof_varint(*(&self.rail9) as u64) } + + if self.rail10 == false { 0 } else { 1 + sizeof_varint(*(&self.rail10) as u64) } + + if self.rail11 == false { 0 } else { 1 + sizeof_varint(*(&self.rail11) as u64) } + + if self.rail12 == false { 0 } else { 1 + sizeof_varint(*(&self.rail12) as u64) } + + if self.rail13 == false { 0 } else { 1 + sizeof_varint(*(&self.rail13) as u64) } + + if self.rail14 == false { 0 } else { 1 + sizeof_varint(*(&self.rail14) as u64) } + + if self.rail15 == false { 0 } else { 1 + sizeof_varint(*(&self.rail15) as u64) } + + if self.rail16 == false { 0 } else { 2 + sizeof_varint(*(&self.rail16) as u64) } + + if self.hpwr1 == false { 0 } else { 2 + sizeof_varint(*(&self.hpwr1) as u64) } + + if self.hpwr2 == false { 0 } else { 2 + sizeof_varint(*(&self.hpwr2) as u64) } + + if self.hpwrEn == false { 0 } else { 2 + sizeof_varint(*(&self.hpwrEn) as u64) } + } + + fn write_message(&self, w: &mut Writer) -> Result<()> { + if self.rail1 != false { w.write_with_tag(8, |w| w.write_bool(*&self.rail1))?; } + if self.rail2 != false { w.write_with_tag(16, |w| w.write_bool(*&self.rail2))?; } + if self.rail3 != false { w.write_with_tag(24, |w| w.write_bool(*&self.rail3))?; } + if self.rail4 != false { w.write_with_tag(32, |w| w.write_bool(*&self.rail4))?; } + if self.rail5 != false { w.write_with_tag(40, |w| w.write_bool(*&self.rail5))?; } + if self.rail6 != false { w.write_with_tag(48, |w| w.write_bool(*&self.rail6))?; } + if self.rail7 != false { w.write_with_tag(56, |w| w.write_bool(*&self.rail7))?; } + if self.rail8 != false { w.write_with_tag(64, |w| w.write_bool(*&self.rail8))?; } + if self.rail9 != false { w.write_with_tag(72, |w| w.write_bool(*&self.rail9))?; } + if self.rail10 != false { w.write_with_tag(80, |w| w.write_bool(*&self.rail10))?; } + if self.rail11 != false { w.write_with_tag(88, |w| w.write_bool(*&self.rail11))?; } + if self.rail12 != false { w.write_with_tag(96, |w| w.write_bool(*&self.rail12))?; } + if self.rail13 != false { w.write_with_tag(104, |w| w.write_bool(*&self.rail13))?; } + if self.rail14 != false { w.write_with_tag(112, |w| w.write_bool(*&self.rail14))?; } + if self.rail15 != false { w.write_with_tag(120, |w| w.write_bool(*&self.rail15))?; } + if self.rail16 != false { w.write_with_tag(128, |w| w.write_bool(*&self.rail16))?; } + if self.hpwr1 != false { w.write_with_tag(136, |w| w.write_bool(*&self.hpwr1))?; } + if self.hpwr2 != false { w.write_with_tag(144, |w| w.write_bool(*&self.hpwr2))?; } + if self.hpwrEn != false { w.write_with_tag(152, |w| w.write_bool(*&self.hpwrEn))?; } + Ok(()) + } +} + +#[derive(Debug, Default, PartialEq, Clone)] +pub struct RadioSOH { + pub batteryVoltage: Option, + pub solarVoltage: Option, + pub batteryVoltageState: protos::no_std::BatteryVoltageState, + pub batteryManagerStates: Option, + pub railSoh: Option, +} + +impl<'a> MessageRead<'a> for RadioSOH { + fn from_reader(r: &mut BytesReader, bytes: &'a [u8]) -> Result { + let mut msg = Self::default(); + while !r.is_eof() { + match r.next_tag(bytes) { + Ok(10) => msg.batteryVoltage = Some(r.read_message::(bytes)?), + Ok(18) => msg.solarVoltage = Some(r.read_message::(bytes)?), + Ok(24) => msg.batteryVoltageState = r.read_enum(bytes)?, + Ok(34) => msg.batteryManagerStates = Some(r.read_message::(bytes)?), + Ok(42) => msg.railSoh = Some(r.read_message::(bytes)?), + Ok(t) => { r.read_unknown(bytes, t)?; } + Err(e) => return Err(e), + } + } + Ok(msg) + } +} + +impl MessageWrite for RadioSOH { + fn get_size(&self) -> usize { + 0 + + self.batteryVoltage.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + + self.solarVoltage.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + + if self.batteryVoltageState == protos::no_std::BatteryVoltageState::BothHigh { 0 } else { 1 + sizeof_varint(*(&self.batteryVoltageState) as u64) } + + self.batteryManagerStates.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + + self.railSoh.as_ref().map_or(0, |m| 1 + sizeof_len((m).get_size())) + } + + fn write_message(&self, w: &mut Writer) -> Result<()> { + if let Some(ref s) = self.batteryVoltage { w.write_with_tag(10, |w| w.write_message(s))?; } + if let Some(ref s) = self.solarVoltage { w.write_with_tag(18, |w| w.write_message(s))?; } + if self.batteryVoltageState != protos::no_std::BatteryVoltageState::BothHigh { w.write_with_tag(24, |w| w.write_enum(*&self.batteryVoltageState as i32))?; } + if let Some(ref s) = self.batteryManagerStates { w.write_with_tag(34, |w| w.write_message(s))?; } + if let Some(ref s) = self.railSoh { w.write_with_tag(42, |w| w.write_message(s))?; } + Ok(()) + } +} + +#[derive(Debug, Default, PartialEq, Clone)] +pub struct RadioTelemetry { + pub tid: protos::no_std::TelemetryID, + pub message: protos::no_std::mod_RadioTelemetry::OneOfmessage, +} + +impl<'a> MessageRead<'a> for RadioTelemetry { + fn from_reader(r: &mut BytesReader, bytes: &'a [u8]) -> Result { + let mut msg = Self::default(); + while !r.is_eof() { + match r.next_tag(bytes) { + Ok(8) => msg.tid = r.read_enum(bytes)?, + Ok(18) => msg.message = protos::no_std::mod_RadioTelemetry::OneOfmessage::soh(r.read_message::(bytes)?), + Ok(t) => { r.read_unknown(bytes, t)?; } + Err(e) => return Err(e), + } + } + Ok(msg) + } +} + +impl MessageWrite for RadioTelemetry { + fn get_size(&self) -> usize { + 0 + + if self.tid == protos::no_std::TelemetryID::SOH { 0 } else { 1 + sizeof_varint(*(&self.tid) as u64) } + + match self.message { + protos::no_std::mod_RadioTelemetry::OneOfmessage::soh(ref m) => 1 + sizeof_len((m).get_size()), + protos::no_std::mod_RadioTelemetry::OneOfmessage::None => 0, + } } + + fn write_message(&self, w: &mut Writer) -> Result<()> { + if self.tid != protos::no_std::TelemetryID::SOH { w.write_with_tag(8, |w| w.write_enum(*&self.tid as i32))?; } + match self.message { protos::no_std::mod_RadioTelemetry::OneOfmessage::soh(ref m) => { w.write_with_tag(18, |w| w.write_message(m))? }, + protos::no_std::mod_RadioTelemetry::OneOfmessage::None => {}, + } Ok(()) + } +} + +pub mod mod_RadioTelemetry { + +use super::*; + +#[derive(Debug, PartialEq, Clone)] +pub enum OneOfmessage { + soh(protos::no_std::RadioSOH), + None, +} + +impl Default for OneOfmessage { + fn default() -> Self { + OneOfmessage::None + } +} + +} +