diff --git a/firmware/src/bus.rs b/firmware/src/bus.rs index d8103d7..c2f71dc 100644 --- a/firmware/src/bus.rs +++ b/firmware/src/bus.rs @@ -20,17 +20,17 @@ use iocan_proto::{TpdoFrame, TpdoKind, decode_pdo}; use num_traits::float::Float; use zencan_common::{CanId, CanMessage, sdo::SdoRequest}; -use rapid_dialect::rapid::enums::{ValveId, valve_id}; +use rapid_dialect::rapid::enums::{MavBatteryChargeState, ValveId, valve_id}; use mission::bus::{ Bus, BusDataError, BusInputImage, BusOutputImage, DataWithTime, IoAddr, NODE_ID_COUNT, - ValveState, + POWER_BOARD_NODE_IDS, ValveState, power_board, }; use mission::inventory::{ - BinaryOutputId, BinaryOutputMap, InventoryId, ServoId, ServoMap, ValveMap, + BinaryOutputId, BinaryOutputMap, InventoryId, PowerBoardId, ServoId, ServoMap, ValveMap, }; -use crate::bus::mapping::{BINARY_OUTPUT_ID_MAP, SERVO_ID_MAP, VALVE_ID_MAP}; +use crate::bus::mapping::{BINARY_OUTPUT_ID_MAP, SERVO_ID_MAP, VALVE_ID_MAP, charger_bits}; use crate::bus::pdo_mapping::{ SensorReading, hco_msg_to_binary_outputs, sensor_msg_to_readings, valve_msg_to_servo, valve_msg_to_valve, @@ -97,6 +97,12 @@ impl Bus for BusHandler { // A board gone quiet would otherwise keep claiming its last rail reading. self.input.nodes_armed = self.input.nodes_armed.intersection(self.input.nodes); + let nodes = self.input.nodes; + self.input.power_boards.update(|id, reading| { + if !nodes.contains(POWER_BOARD_NODE_IDS[id]) { + *reading = None; + } + }); self.input.clone() } @@ -214,7 +220,14 @@ fn try_injest_can_msg(image: &mut BusInputImage, frame: Frame, time: Wrapping { @@ -277,6 +290,36 @@ fn try_injest_can_msg(image: &mut BusInputImage, frame: Frame, time: Wrapping reading.voltage_mv = Some(pack_mv), + TpdoFrame::RailCurrent([charge_ma, discharge_ma, _]) => { + reading.current_ma = Some(i32::from(discharge_ma) - i32::from(charge_ma)); + } + // `raw_debug` is the charger's I2C health; without it every other field is stale. + TpdoFrame::Status { + raw_debug: i2c_ok, + stalled_mask: bits, + .. + } => { + let phase = + (bits & charger_bits::CHARGE_STATE_MASK) >> charger_bits::CHARGE_STATE_SHIFT; + reading.charge_state = if !i2c_ok { + MavBatteryChargeState::Undefined + } else if bits & charger_bits::FAULT != 0 { + MavBatteryChargeState::Failed + } else if (1..=6).contains(&phase) { + MavBatteryChargeState::Charging + } else { + MavBatteryChargeState::Ok + }; + } + _ => (), + } +} + // technically const /// Convert between different CanMessage types pub fn can_msg_to_frame(msg: &CanMessage) -> embassy_stm32::can::Frame { diff --git a/firmware/src/bus/mapping.rs b/firmware/src/bus/mapping.rs index 11bf47d..b840744 100644 --- a/firmware/src/bus/mapping.rs +++ b/firmware/src/bus/mapping.rs @@ -127,3 +127,12 @@ pub const PRESS_SENSOR_ID_MAP: PressureSensorMap = PressureSensorMap slot: 0xff, }, // PressSensId::ExternalOxidizer ]); + +/// The power boards' `Status` frame repurposes `stalled_mask` for charger state. Restated from +/// power_board_firmware's `src/can/tpdo.rs`. +pub mod charger_bits { + pub const FAULT: u8 = 1 << 0; + /// REG1C.CHG_STAT: 0 not charging, 1..=6 charging phases, 7 done. + pub const CHARGE_STATE_SHIFT: u8 = 4; + pub const CHARGE_STATE_MASK: u8 = 0b0111 << CHARGE_STATE_SHIFT; +} diff --git a/mission/src/bus.rs b/mission/src/bus.rs index 4c34941..cad7e82 100644 --- a/mission/src/bus.rs +++ b/mission/src/bus.rs @@ -1,7 +1,10 @@ use core::num::Wrapping; +use rapid_dialect::rapid::enums::MavBatteryChargeState; + use crate::inventory::{ - BinaryOutputMap, OxProbeMap, PressureSensorMap, ServoMap, TemperatureSensorMap, ValveMap, + BinaryOutputMap, InventoryId, OxProbeMap, PowerBoardId, PowerBoardMap, PressureSensorMap, + ServoMap, TemperatureSensorMap, ValveMap, }; pub trait Bus { @@ -45,6 +48,17 @@ pub struct BusInputImage { pub nodes: NodeSet, /// A subset of `nodes`: a board we cannot hear from tells us nothing. pub nodes_armed: NodeSet, + /// `None` while the board is not present. + pub power_boards: PowerBoardMap>, +} + +/// One power board's battery pack. Each field arrives in a frame of its own. +#[derive(Clone, Copy, Default)] +pub struct PowerBoardReading { + pub voltage_mv: Option, + /// Positive while discharging. + pub current_ma: Option, + pub charge_state: MavBatteryChargeState, } /// The IO board protocol's node id field is four bits wide. @@ -70,6 +84,15 @@ pub const IO_NODE_IDS: [u8; NODE_ID_COUNT - 2] = { ids }; +/// As flashed by power_board_firmware's `justfile`. +pub const POWER_BOARD_NODE_IDS: PowerBoardMap = PowerBoardMap::new([11, 12, 13]); + +pub fn power_board(node_id: u8) -> Option { + PowerBoardId::ALL + .into_iter() + .find(|&id| POWER_BOARD_NODE_IDS[id] == node_id) +} + /// A set of IO board node ids. Bit n is node id n, which is also the LoRa downlink's encoding, /// so [`Self::bits`] goes straight onto the wire. #[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] @@ -169,6 +192,7 @@ impl BusInputImage { valve_temp: None, nodes: NodeSet::NONE, nodes_armed: NodeSet::NONE, + power_boards: PowerBoardMap::splat(None), } } } diff --git a/mission/src/inventory.rs b/mission/src/inventory.rs index 7f2e25d..c6ac51d 100644 --- a/mission/src/inventory.rs +++ b/mission/src/inventory.rs @@ -83,6 +83,15 @@ pub enum ServoId { OxidizerRetract, } +/// The battery power boards on the vehicle bus. +#[derive(Copy, Clone, Eq, PartialEq, Debug)] +#[repr(u8)] +pub enum PowerBoardId { + Board1, + Board2, + Board3, +} + /// A fixed-size array indexed by an id enum instead of a raw usize. pub struct InventoryMap { values: [T; N], @@ -96,6 +105,7 @@ pub type PressureSensorMap = InventoryMap; pub type BinaryOutputMap = InventoryMap; pub type TankMap = InventoryMap; pub type ServoMap = InventoryMap; +pub type PowerBoardMap = InventoryMap; /// An id enum that can key an [`InventoryMap`]: N variants, each mapping to a unique dense index /// in 0..N. @@ -347,6 +357,14 @@ impl InventoryId<4> for ServoId { } } +impl InventoryId<3> for PowerBoardId { + const ALL: [Self; 3] = [Self::Board1, Self::Board2, Self::Board3]; + + fn idx(self) -> usize { + self as usize + } +} + impl InventoryId<6> for TankId { const ALL: [Self; 6] = [ Self::Pressurant, @@ -496,6 +514,7 @@ mod tests { check::(); check::(); check::(); + check::(); } /// The two halves of the inventory travel in separate telemetry messages, so a tank whose diff --git a/mission/src/mavlink.rs b/mission/src/mavlink.rs index 596afb0..0cd80c5 100644 --- a/mission/src/mavlink.rs +++ b/mission/src/mavlink.rs @@ -24,11 +24,22 @@ use state_estimator::StateEstimator; use crate::TelemetryLink; use crate::bus::{BusInputImage, BusOutputImage, ValveState}; -use crate::inventory::{InventoryId, OxProbeId, ServoMap, TankId, ValveId, valve_is_heated}; +use crate::inventory::{ + InventoryId, OxProbeId, PowerBoardId, ServoMap, TankId, ValveId, valve_is_heated, +}; use crate::params::StateMachineParams; use crate::schedule::downlink_schedule; use crate::traits::SensorReadings; +/// `None` is the flight computer's own power monitor, which only reports while no power board is +/// present. +const BATTERY_SOURCES: [Option; 4] = [ + None, + Some(PowerBoardId::Board1), + Some(PowerBoardId::Board2), + Some(PowerBoardId::Board3), +]; + /// Everything the vehicle exposes about one tick, borrowed rather than copied. Built by /// `Vehicle::snapshot`; the conversions in this module read nothing else. pub struct VehicleSnapshot<'a> { @@ -49,7 +60,7 @@ impl VehicleSnapshot<'_> { pub fn send_telemetry(&self, link: &mut impl TelemetryLink) { downlink_schedule! { self.time.0, self, link: every 100 ms => Attitude, VfrHud, ScaledImu, ScaledImu2, ScaledImu3; - every 200 ms => BatteryStatus, LocalPositionNed, + every 200 ms => BatteryStatus[BATTERY_SOURCES], LocalPositionNed, ScaledPressure, ScaledPressure2, ScaledPressure3; every 500 ms => Heartbeat, SysStatus, GlobalPositionInt, GpsRawInt, DebugFloatArray; every 2000 ms => RocketInfo, AutopilotVersion; @@ -526,51 +537,6 @@ impl Into for &VehicleSnapshot<'_> { } } -impl Into for &VehicleSnapshot<'_> { - #[allow( - clippy::arithmetic_side_effects, - reason = "bounded i32 sensor math with nonzero constant divisors, cannot over/underflow or divide by zero" - )] - fn into(self) -> BatteryStatus { - const CELLS: usize = 3; - - let adc = self.readings.power.as_ref(); - - let mut voltages: [u16; 10] = [u16::MAX; 10]; - if let Some(pack_mv) = adc.map(|d| d.bus_main_voltage) { - voltages[0] = pack_mv; - } - - let current_battery = adc - .map(|d| (d.fc_current / 10).clamp(i16::MIN as i32, i16::MAX as i32) as i16) - .unwrap_or(-1); - - let battery_remaining = adc - .map(|d| { - let cell_mv = i32::from(d.bus_main_voltage) / CELLS as i32; - (((cell_mv - 3300) * 100) / (4200 - 3300)).clamp(0, 100) as i8 - }) - .unwrap_or(-1); - - BatteryStatus { - id: 0x01, - type_: MavBatteryType::Lion, - battery_function: MavBatteryFunction::Avionics, - temperature: i16::MAX, - voltages, - current_battery, - current_consumed: -1, - energy_consumed: -1, - battery_remaining, - time_remaining: 0, - charge_state: MavBatteryChargeState::Undefined, - voltages_ext: [u16::MAX; 4], - mode: MavBatteryMode::Unknown, - fault_bitmask: MavBatteryFault::default(), - } - } -} - /// A message the flight computer sends one of per component. `None` leaves that slot silent. trait InstanceMessage: Sized { fn component_id(_id: I) -> u8 { @@ -594,10 +560,15 @@ pub fn autopilot_version() -> AutopilotVersion { /// Shared because the ground station rebuilds these from the LoRa downlink, and both paths have /// to produce the same message. -pub fn io_node_heartbeat(armed: bool) -> Heartbeat { +pub fn io_node_heartbeat(node_id: u8, armed: bool) -> Heartbeat { Heartbeat { - // The closest MAV_TYPE to an io board; the spec identifies components by type, not by id. - type_: MavType::Servo, + // The spec identifies components by type, not by id. Servo is the closest MAV_TYPE to an + // io board. + type_: if crate::bus::power_board(node_id).is_some() { + MavType::Battery + } else { + MavType::Servo + }, autopilot: MavAutopilot::Invalid, base_mode: if armed { MavModeFlag::SAFETY_ARMED @@ -622,7 +593,94 @@ impl InstanceMessage for Heartbeat { snap.input_image .nodes .contains(node_id) - .then(|| io_node_heartbeat(snap.input_image.nodes_armed.contains(node_id))) + .then(|| io_node_heartbeat(node_id, snap.input_image.nodes_armed.contains(node_id))) + } +} + +impl InstanceMessage> for BatteryStatus { + fn build(snap: &VehicleSnapshot<'_>, source: Option) -> Option { + let boards = &snap.input_image.power_boards; + + let (id, voltage_mv, current_ma, charge_state) = match source { + Some(board) => { + let reading = boards[board]?; + ( + battery_id(board), + reading.voltage_mv, + reading.current_ma, + reading.charge_state, + ) + } + None => { + if boards.iter().any(|(_, reading)| reading.is_some()) { + return None; + } + let adc = snap.readings.power.as_ref(); + ( + 1, + adc.map(|d| d.bus_main_voltage), + adc.map(|d| d.fc_current), + MavBatteryChargeState::Undefined, + ) + } + }; + + Some(battery_status(id, voltage_mv, current_ma, charge_state)) + } +} + +pub fn battery_id(board: PowerBoardId) -> u8 { + match board { + PowerBoardId::Board1 => 1, + PowerBoardId::Board2 => 2, + PowerBoardId::Board3 => 3, + } +} + +/// A 3S Li-ion pack. +#[allow( + clippy::arithmetic_side_effects, + reason = "bounded i32 sensor math with nonzero constant divisors, cannot over/underflow or divide by zero" +)] +pub fn battery_status( + id: u8, + voltage_mv: Option, + current_ma: Option, + charge_state: MavBatteryChargeState, +) -> BatteryStatus { + const CELLS: i32 = 3; + + let mut voltages: [u16; 10] = [u16::MAX; 10]; + if let Some(pack_mv) = voltage_mv { + voltages[0] = pack_mv; + } + + let current_battery = current_ma + .map(|ma| (ma / 10).clamp(i16::MIN as i32, i16::MAX as i32) as i16) + .unwrap_or(-1); + + let battery_remaining = voltage_mv + .map(|mv| { + let cell_mv = i32::from(mv) / CELLS; + (((cell_mv - 3300) * 100) / (4200 - 3300)).clamp(0, 100) as i8 + }) + .unwrap_or(-1); + + BatteryStatus { + id, + type_: MavBatteryType::Lion, + battery_function: MavBatteryFunction::Avionics, + temperature: i16::MAX, + voltages, + current_battery, + current_consumed: -1, + energy_consumed: -1, + battery_remaining, + time_remaining: 0, + charge_state, + voltages_ext: [u16::MAX; 4], + mode: MavBatteryMode::Unknown, + fault_bitmask: MavBatteryFault::default(), } } diff --git a/mission/src/params.rs b/mission/src/params.rs index bd218ae..f29ab58 100644 --- a/mission/src/params.rs +++ b/mission/src/params.rs @@ -359,7 +359,7 @@ mod tests { assert_eq!(s.state_machine.main_deploy_altitude, 400.0); assert_eq!(s.state_machine.min_time_to_drogue, 1000); assert_eq!(s.misc.buzzer_volume, 50); - assert_eq!(s.state_machine.main_on_time, 500); + assert_eq!(s.state_machine.main_on_time, 300); assert_eq!(s.state_machine.main_pulses, 2); assert_eq!(s.qd.disconnect_retract_pressurant, 10); } diff --git a/sitl/src/simulation.rs b/sitl/src/simulation.rs index fedee8b..49d39f6 100644 --- a/sitl/src/simulation.rs +++ b/sitl/src/simulation.rs @@ -1,5 +1,6 @@ use std::sync::{Arc, Mutex}; +use mission::inventory::{PowerBoardId, PowerBoardMap}; use rapid_dialect::FlightMode; pub mod battery; @@ -22,7 +23,8 @@ use hybrid::HybridSimulation; pub struct Simulation { pub physics: FlightPhysics, - pub battery: Battery, + /// One pack per power board. + pub batteries: PowerBoardMap, #[cfg(feature = "hybrid")] pub hybrid: HybridSimulation, } @@ -33,7 +35,7 @@ impl Simulation { pub fn new(flags: RecoveryFlags) -> Self { Self { physics: FlightPhysics::new(flags), - battery: Battery::new(), + batteries: new_batteries(), #[cfg(feature = "hybrid")] hybrid: HybridSimulation::new(), } @@ -46,7 +48,7 @@ impl Simulation { self.hybrid.set_flight_mode(mode); if mode == FlightMode::Idle && prev != FlightMode::Idle { - self.battery = Battery::new(); + self.batteries = new_batteries(); #[cfg(feature = "hybrid")] { self.hybrid = HybridSimulation::new(); @@ -56,7 +58,8 @@ impl Simulation { pub fn tick(&mut self) { self.physics.tick(); - self.battery.tick(DT, self.physics.mode); + let mode = self.physics.mode; + self.batteries.update(|_, battery| battery.tick(DT, mode)); #[cfg(feature = "hybrid")] self.hybrid.tick(DT); @@ -65,3 +68,14 @@ impl Simulation { .set_chamber_pressure(self.hybrid.chamber_pressure); } } + +/// Charged differently, so the packs can be told apart. The levels are arbitrary. +fn new_batteries() -> PowerBoardMap { + PowerBoardMap::from_fn(|id| { + Battery::new(match id { + PowerBoardId::Board1 => 0.90, + PowerBoardId::Board2 => 0.80, + PowerBoardId::Board3 => 0.70, + }) + }) +} diff --git a/sitl/src/simulation/battery.rs b/sitl/src/simulation/battery.rs index 6fb46bb..b957caa 100644 --- a/sitl/src/simulation/battery.rs +++ b/sitl/src/simulation/battery.rs @@ -31,15 +31,8 @@ pub struct Battery { pub voltage: f32, } -impl Default for Battery { - fn default() -> Self { - Self::new() - } -} - impl Battery { - pub fn new() -> Self { - let soc = 0.90; + pub fn new(soc: f32) -> Self { Self { soc, current: 0.0, diff --git a/sitl/src/simulation/hybrid.rs b/sitl/src/simulation/hybrid.rs index a1a65ba..1f60af1 100644 --- a/sitl/src/simulation/hybrid.rs +++ b/sitl/src/simulation/hybrid.rs @@ -6,13 +6,16 @@ use std::num::Wrapping; -use mission::bus::{Bus, BusInputImage, BusOutputImage, DataWithTime, NodeSet, ValveState}; +use mission::bus::{ + Bus, BusInputImage, BusOutputImage, DataWithTime, NodeSet, POWER_BOARD_NODE_IDS, + PowerBoardReading, ValveState, +}; use rapid_dialect::FlightMode; -use rapid_dialect::rapid::enums::ValveId; +use rapid_dialect::rapid::enums::{MavBatteryChargeState, ValveId}; use mission::inventory::{ - BinaryOutputId, BinaryOutputMap, InventoryId, OxProbeId, OxProbeMap, PressSensId, - PressureSensorMap, ServoMap, TankId, TemperatureSensorMap, ValveMap, + BinaryOutputId, BinaryOutputMap, InventoryId, OxProbeId, OxProbeMap, PowerBoardMap, + PressSensId, PressureSensorMap, ServoMap, TankId, TemperatureSensorMap, ValveMap, }; use mission::valves::ValveCommand; @@ -361,11 +364,30 @@ impl Bus for SitlBus { )) }); - let mut nodes = NodeSet::NONE; + let mut io_nodes = NodeSet::NONE; for node_id in ONBOARD_NODES { + io_nodes.set(node_id, true); + } + io_nodes.set(UMBILICAL_NODE, umbilical_connected); + + let mut nodes = io_nodes; + // No high-current rail, so never armed. + for &node_id in POWER_BOARD_NODE_IDS.values() { nodes.set(node_id, true); } - nodes.set(UMBILICAL_NODE, umbilical_connected); + + let power_boards = PowerBoardMap::from_fn(|id| { + let battery = &sim.batteries[id]; + Some(PowerBoardReading { + voltage_mv: Some((battery.voltage * 1000.0) as u16), + current_ma: Some((battery.current * 1000.0) as i32), + charge_state: if battery.current < 0.0 { + MavBatteryChargeState::Charging + } else { + MavBatteryChargeState::Ok + }, + }) + }); let ox_probes = OxProbeMap::from_fn(|id| { Some(DataWithTime::new(sim.hybrid.probe_temperature(id), now)) @@ -386,8 +408,9 @@ impl Bus for SitlBus { nodes_armed: if sim.hybrid.flight_mode == FlightMode::Idle { NodeSet::NONE } else { - nodes + io_nodes }, + power_boards, } } diff --git a/sitl/src/simulation/sensors.rs b/sitl/src/simulation/sensors.rs index 039b001..93edbb7 100644 --- a/sitl/src/simulation/sensors.rs +++ b/sitl/src/simulation/sensors.rs @@ -12,6 +12,7 @@ use nalgebra::Vector3; use rand::Rng; use rapid_dialect::FlightMode; +use mission::inventory::PowerBoardId; use mission::{AdcData, BaroReading, SensorReadings, Sensors}; use state_estimator::GpsDatum; @@ -302,6 +303,8 @@ impl StdSensors { impl Sensors for StdSensors { async fn tick(&mut self) -> SensorReadings { let sim = self.sim.lock().unwrap(); - self.sensor_model.sample(&sim.physics, &sim.battery) + // Which pack feeds the flight computer is arbitrary. + self.sensor_model + .sample(&sim.physics, &sim.batteries[PowerBoardId::Board1]) } } diff --git a/sitl/tests/telemetry.rs b/sitl/tests/telemetry.rs index eb15a53..07144fe 100644 --- a/sitl/tests/telemetry.rs +++ b/sitl/tests/telemetry.rs @@ -12,7 +12,7 @@ use links::SELF_COMPONENT_ID; use mission::TankId; use mission::inventory::InventoryId; use rapid_dialect::Rapid; -use rapid_dialect::rapid::enums::{MavModeFlag, ValveId}; +use rapid_dialect::rapid::enums::{MavModeFlag, MavType, ValveId}; /// Two full cycles of the slowest (2000 ms) interval, so every combination of phases that can /// coincide has had the chance to. Every interval in the schedule has to divide this, or the @@ -39,6 +39,12 @@ macro_rules! assert_rates { }; } +/// One per simulated power board, or the flight computer's own when there are none. +#[cfg(feature = "hybrid")] +const BATTERY_IDS: [u8; 3] = [1, 2, 3]; +#[cfg(not(feature = "hybrid"))] +const BATTERY_IDS: [u8; 1] = [1]; + #[test] fn every_message_goes_out_at_its_intended_rate() { block_on(async { @@ -58,7 +64,6 @@ fn every_message_goes_out_at_its_intended_rate() { ScaledImu every 100, ScaledImu2 every 100, ScaledImu3 every 100, - BatteryStatus every 200, LocalPositionNed every 200, ScaledPressure every 200, ScaledPressure2 every 200, @@ -87,6 +92,25 @@ fn every_message_goes_out_at_its_intended_rate() { ); } + for id in BATTERY_IDS { + let reports = sent + .iter() + .filter(|m| { + m.component_id == SELF_COMPONENT_ID + && matches!(&m.message, Rapid::BatteryStatus(b) if b.id == id) + }) + .count(); + assert_eq!( + reports, propulsion_reports, + "battery {id} reported {reports} times" + ); + } + let batteries = sent + .iter() + .filter(|m| matches!(m.message, Rapid::BatteryStatus(_))) + .count(); + assert_eq!(batteries, BATTERY_IDS.len() * propulsion_reports); + for valve in ValveId::ALL { let reports = sent .iter() @@ -106,6 +130,11 @@ fn every_message_goes_out_at_its_intended_rate() { const SIMULATED_NODES: [u8; 7] = [2, 3, 4, 5, 6, 7, 8]; #[cfg(not(feature = "hybrid"))] const SIMULATED_NODES: [u8; 0] = []; +/// Heartbeat like the io boards, but have no high-current rail to arm. +#[cfg(feature = "hybrid")] +const SIMULATED_POWER_BOARD_NODES: [u8; 3] = [11, 12, 13]; +#[cfg(not(feature = "hybrid"))] +const SIMULATED_POWER_BOARD_NODES: [u8; 0] = []; /// A publisher that forgets which component it speaks for would show up as a phantom board. #[test] @@ -142,14 +171,20 @@ fn every_present_io_board_node_heartbeats_as_its_own_component() { } for message in sent.iter().filter(|m| m.component_id != SELF_COMPONENT_ID) { + let expected_type = if SIMULATED_POWER_BOARD_NODES.contains(&message.component_id) { + MavType::Battery + } else { + MavType::Servo + }; assert!( - matches!(message.message, Rapid::Heartbeat(_)), - "component {} sent something other than a heartbeat", + matches!(&message.message, Rapid::Heartbeat(h) if h.type_ == expected_type), + "component {} sent something other than a {expected_type:?} heartbeat", message.component_id ); assert!( - SIMULATED_NODES.contains(&message.component_id), - "component {} is not a simulated io board node", + SIMULATED_NODES.contains(&message.component_id) + || SIMULATED_POWER_BOARD_NODES.contains(&message.component_id), + "component {} is not a simulated bus node", message.component_id ); } diff --git a/telemetry/src/messages/downlink.rs b/telemetry/src/messages/downlink.rs index cfeea42..60132ba 100644 --- a/telemetry/src/messages/downlink.rs +++ b/telemetry/src/messages/downlink.rs @@ -16,6 +16,7 @@ use crate::DOWNLINK_MESSAGE_INTERVAL_MS; use crate::TelemetryError; use crate::messages::TelemetryMessage; +mod battery; mod components; mod gps; mod heartbeat; @@ -24,6 +25,7 @@ mod pressures; mod sensors; mod status; +pub use battery::BatteryMessage; pub use components::ComponentsMessage; pub use gps::GpsMessage; pub use heartbeat::HeartbeatMessage; @@ -170,6 +172,7 @@ pub enum DownlinkMessage { Sensors(SensorsMessage), Gps(GpsMessage), ParamValues(ParamValuesMessage), + Battery(BatteryMessage), } impl DownlinkMessage { @@ -207,12 +210,14 @@ impl DownlinkMessage { )] let slot = (time_ms / DOWNLINK_MESSAGE_INTERVAL_MS) % Self::SLOT_COUNT; - // The four packed messages repeat over each half of the cycle, so the GPS and external - // pressure slots are paid for out of the heartbeat's share alone. + // The packed messages repeat over each half of the cycle, so the GPS and external + // pressure slots are paid for out of the heartbeat's share alone. Status changes slowest, + // so its second slot goes to the batteries instead. Some(match slot { 1 | 9 => Self::Pressures(PressuresMessage::pack(snapshot)), 3 | 11 => Self::Components(ComponentsMessage::pack(snapshot)), - 5 | 13 => Self::Status(StatusMessage::pack((snapshot, uplink))), + 5 => Self::Status(StatusMessage::pack((snapshot, uplink))), + 13 => Self::Battery(BatteryMessage::pack(snapshot)), 7 | 15 => Self::Sensors(SensorsMessage::pack(snapshot)), // The umbilical is severed at liftoff, so the ground side hands its slot back. 6 if snapshot.mode < FlightMode::Burn => { @@ -250,6 +255,7 @@ impl TelemetryMessage for DownlinkMessage { Self::Sensors(inner) => (SensorsMessage::ID, inner.serialize()?), Self::Gps(inner) => (GpsMessage::ID, inner.serialize()?), Self::ParamValues(inner) => (ParamValuesMessage::ID, inner.serialize()?), + Self::Battery(inner) => (BatteryMessage::ID, inner.serialize()?), }; let mut buffer = [0x00; DOWNLINK_PACKET_SIZE]; @@ -299,6 +305,7 @@ impl TelemetryMessage for DownlinkMessage { SensorsMessage::ID => DownlinkMessage::Sensors(postcard::from_bytes(payload)?), GpsMessage::ID => DownlinkMessage::Gps(postcard::from_bytes(payload)?), ParamValuesMessage::ID => DownlinkMessage::ParamValues(postcard::from_bytes(payload)?), + BatteryMessage::ID => DownlinkMessage::Battery(postcard::from_bytes(payload)?), id => { return Err(TelemetryError::UnknownMessageId(id)); } @@ -349,7 +356,7 @@ impl TelemetryMessage for DownlinkMessage { // The wired links send these from the schedule; rebuilding them here makes a // board look the same either way. for node_id in IO_NODE_IDS.into_iter().filter(|id| nodes.contains(*id)) { - let heartbeat = io_node_heartbeat(armed.contains(node_id)); + let heartbeat = io_node_heartbeat(node_id, armed.contains(node_id)); sender.anysend(Downlink::new(node_id, heartbeat)).await; } } @@ -368,6 +375,11 @@ impl TelemetryMessage for DownlinkMessage { sender.anysend(Downlink::from_self(value)).await; } } + Self::Battery(inner) => { + for status in inner.unpack(context).into_iter().flatten() { + sender.anysend(Downlink::from_self(status)).await; + } + } } } } @@ -640,7 +652,8 @@ pub(crate) mod tests { let expected = match slot { 1 | 9 => PressuresMessage::ID, 3 | 11 => ComponentsMessage::ID, - 5 | 13 => StatusMessage::ID, + 5 => StatusMessage::ID, + 13 => BatteryMessage::ID, 7 | 15 => SensorsMessage::ID, 6 if mode < FlightMode::Burn => ExternalPressuresMessage::ID, 14 => GpsMessage::ID, @@ -667,7 +680,7 @@ pub(crate) mod tests { /// the bench. This is what catches a field that lost its `fixint` annotation. #[test] fn no_payload_length_depends_on_its_values() { - fn lengths(parts: &SnapshotParts, param: ParamEntry) -> [usize; 8] { + fn lengths(parts: &SnapshotParts, param: ParamEntry) -> [usize; 9] { let s = parts.snapshot(); [ encoded_len(&HeartbeatMessage::pack((&s, CommandAck::NONE))), @@ -678,6 +691,7 @@ pub(crate) mod tests { encoded_len(&SensorsMessage::pack(&s)), encoded_len(&GpsMessage::pack(&s)), encoded_len(&ParamValuesMessage::pack([param; 2])), + encoded_len(&BatteryMessage::pack(&s)), ] } @@ -687,6 +701,7 @@ pub(crate) mod tests { // Every reading present and large, which is where varints would have grown. parts.readings = sensors::tests::saturated_readings(); parts.inputs = pressures::tests::saturated_inputs(); + battery::tests::saturate_power_boards(&mut parts.inputs); parts.outputs = components::tests::saturated_outputs(); parts.estimator = heartbeat::tests::flying_estimator(); // After the readings, which the line above replaces wholesale. diff --git a/telemetry/src/messages/downlink/battery.rs b/telemetry/src/messages/downlink/battery.rs new file mode 100644 index 0000000..00b82fd --- /dev/null +++ b/telemetry/src/messages/downlink/battery.rs @@ -0,0 +1,208 @@ +use serde::{Deserialize, Serialize}; + +use mission::bus::PowerBoardReading; +use mission::inventory::{InventoryId, PowerBoardId}; +use mission::mavlink::{VehicleSnapshot, battery_id, battery_status}; +use rapid_dialect::rapid::enums::MavBatteryChargeState; +use rapid_dialect::rapid::messages::BatteryStatus; + +use super::{ConnectionContext, DownlinkTelemetryMessage, FULL_SCALE, UNKNOWN}; + +/// 5.00 - 15.16 V at 40 mV, which covers a 3S pack from flat to full with room for a charger's +/// overshoot. +const VOLTAGE_OFFSET_MV: u16 = 5_000; +const VOLTAGE_MV_PER_CODE: u16 = 40; + +/// Matches BATTERY_STATUS.current_battery, so nothing is lost on the way to MAVLink. +const CURRENT_MA_PER_CODE: i32 = 10; +/// -1 is a real reading at this scale, so unknown needs a code of its own. +const CURRENT_UNKNOWN: i16 = i16::MIN; + +/// No reading from this board at all, as opposed to one whose charger state is undefined. +const BOARD_ABSENT: u8 = u8::MAX; + +#[derive(Copy, Clone, Debug, Serialize, Deserialize)] +struct PackedBoard { + voltage: u8, + /// Positive while discharging. + #[serde(with = "postcard::fixint::le")] + current: i16, + /// [`MavBatteryChargeState`], or [`BOARD_ABSENT`]. + charge_state: u8, +} + +impl PackedBoard { + #[allow( + clippy::arithmetic_side_effects, + reason = "saturating subtraction, nonzero constant divisors" + )] + fn pack(reading: Option) -> Self { + let Some(reading) = reading else { + return Self { + voltage: UNKNOWN, + current: CURRENT_UNKNOWN, + charge_state: BOARD_ABSENT, + }; + }; + + let voltage = match reading.voltage_mv { + Some(mv) => (mv.saturating_sub(VOLTAGE_OFFSET_MV) / VOLTAGE_MV_PER_CODE) + .min(FULL_SCALE as u16) as u8, + None => UNKNOWN, + }; + + // One below i16::MIN would be unknown, so the clamp starts above it. + let current = match reading.current_ma { + Some(ma) => (ma / CURRENT_MA_PER_CODE) + .clamp(i32::from(CURRENT_UNKNOWN) + 1, i32::from(i16::MAX)) + as i16, + None => CURRENT_UNKNOWN, + }; + + Self { + voltage, + current, + charge_state: reading.charge_state as u8, + } + } + + #[allow(clippy::arithmetic_side_effects, reason = "bounded by the u8/i16 codes")] + fn unpack(self) -> Option { + if self.charge_state == BOARD_ABSENT { + return None; + } + + Some(PowerBoardReading { + voltage_mv: (self.voltage != UNKNOWN) + .then(|| VOLTAGE_OFFSET_MV + u16::from(self.voltage) * VOLTAGE_MV_PER_CODE), + current_ma: (self.current != CURRENT_UNKNOWN) + .then(|| i32::from(self.current) * CURRENT_MA_PER_CODE), + charge_state: MavBatteryChargeState::try_from(self.charge_state).unwrap_or_default(), + }) + } +} + +/// Every power board's pack, so the receiver can rebuild the same BATTERY_STATUS per board that +/// the wired links send. +#[derive(Debug, Serialize, Deserialize)] +pub struct BatteryMessage { + /// Indexed by [`PowerBoardId::idx`]. + boards: [PackedBoard; 3], +} + +impl DownlinkTelemetryMessage for BatteryMessage { + const ID: u8 = 0x09; + type Input<'a> = &'a VehicleSnapshot<'a>; + /// One per [`PowerBoardId`], `None` for a board that is not reporting. + type Output = [Option; 3]; + + fn pack(snapshot: Self::Input<'_>) -> Self { + Self { + boards: PowerBoardId::ALL + .map(|board| PackedBoard::pack(snapshot.input_image.power_boards[board])), + } + } + + fn unpack(self, _context: &mut ConnectionContext) -> Self::Output { + PowerBoardId::ALL.map(|board| { + #[allow(clippy::indexing_slicing, reason = "idx() is 0..3 by the InventoryId contract")] + let reading = self.boards[board.idx()].unpack()?; + Some(battery_status( + battery_id(board), + reading.voltage_mv, + reading.current_ma, + reading.charge_state, + )) + }) + } +} + +#[cfg(test)] +pub(crate) mod tests { + use super::super::tests::{SnapshotParts, through_packet}; + use super::*; + + use mission::bus::BusInputImage; + + use crate::messages::DownlinkMessage; + + /// Every board reporting at the top of its range, for the payload-length check. + pub(crate) fn saturate_power_boards(inputs: &mut BusInputImage) { + for board in PowerBoardId::ALL { + inputs.power_boards[board] = Some(PowerBoardReading { + voltage_mv: Some(u16::MAX), + current_ma: Some(i32::MAX), + charge_state: MavBatteryChargeState::Charging, + }); + } + } + + fn round_trip(parts: &SnapshotParts) -> [Option; 3] { + let msg = DownlinkMessage::Battery(BatteryMessage::pack(&parts.snapshot())); + let DownlinkMessage::Battery(decoded) = through_packet(msg) else { + panic!("decoded as the wrong message") + }; + decoded.unpack(&mut ConnectionContext::init(0)) + } + + #[test] + fn each_board_survives_the_packet() { + let mut parts = SnapshotParts::default(); + parts.inputs.power_boards[PowerBoardId::Board1] = Some(PowerBoardReading { + voltage_mv: Some(12_150), + current_ma: Some(2_400), + charge_state: MavBatteryChargeState::Ok, + }); + parts.inputs.power_boards[PowerBoardId::Board3] = Some(PowerBoardReading { + voltage_mv: Some(11_020), + current_ma: Some(-180), + charge_state: MavBatteryChargeState::Charging, + }); + + let [one, two, three] = round_trip(&parts); + let one = one.expect("board 1 reported"); + let three = three.expect("board 3 reported"); + + assert!(two.is_none(), "board 2 never reported"); + + assert_eq!(one.id, 1); + assert!(one.voltages[0].abs_diff(12_150) < VOLTAGE_MV_PER_CODE); + assert_eq!(one.current_battery, 240); + assert_eq!(one.charge_state, MavBatteryChargeState::Ok); + + assert_eq!(three.id, 3); + assert!(three.voltages[0].abs_diff(11_020) < VOLTAGE_MV_PER_CODE); + assert_eq!(three.current_battery, -18); + assert_eq!(three.charge_state, MavBatteryChargeState::Charging); + } + + /// A board that is on the bus but has not sent every frame yet must say so field by field, + /// rather than come back as a 0 A, 8 V pack. + #[test] + fn missing_fields_stay_missing() { + let mut parts = SnapshotParts::default(); + parts.inputs.power_boards[PowerBoardId::Board2] = Some(PowerBoardReading::default()); + + let [_, status, _] = round_trip(&parts); + let status = status.expect("board 2 is present"); + + assert_eq!(status.voltages[0], u16::MAX); + assert_eq!(status.current_battery, -1); + assert_eq!(status.charge_state, MavBatteryChargeState::Undefined); + } + + /// Out-of-range readings saturate instead of wrapping into a plausible value, and in + /// particular never land on the unknown codes. + #[test] + fn extremes_saturate() { + for (mv, ma) in [(0, i32::MIN), (u16::MAX, i32::MAX)] { + let packed = PackedBoard::pack(Some(PowerBoardReading { + voltage_mv: Some(mv), + current_ma: Some(ma), + charge_state: MavBatteryChargeState::Ok, + })); + assert_ne!(packed.voltage, UNKNOWN, "{mv} mV"); + assert_ne!(packed.current, CURRENT_UNKNOWN, "{ma} mA"); + } + } +}