Multi-Battery

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<u3
         return Some(node_id);
     };
 
-    match TpdoFrame::decode(kind, data) {
+    let frame = TpdoFrame::decode(kind, data);
+
+    if let Some(board) = power_board(node_id) {
+        injest_power_board_frame(image, board, frame);
+        return Some(node_id);
+    }
+
+    match frame {
         // The measured position is where the valve actually is; commanded and target are the
         // node echoing back what it was asked for, which we already know.
         TpdoFrame::ValveMeasured(positions) => {
@@ -277,6 +290,36 @@ fn try_injest_can_msg(image: &mut BusInputImage, frame: Frame, time: Wrapping<u3
     Some(node_id)
 }
 
+fn injest_power_board_frame(image: &mut BusInputImage, board: PowerBoardId, frame: TpdoFrame) {
+    let reading = image.power_boards[board].get_or_insert_default();
+
+    match frame {
+        TpdoFrame::RailVoltage([pack_mv, _, _]) => 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 {

firmware/src/bus/mapping.rs

@@ -127,3 +127,12 @@ pub const PRESS_SENSOR_ID_MAP: PressureSensorMap<SensorAddr> = 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;
+}

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<Option<PowerBoardReading>>,
+}
+
+/// 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<u16>,
+    /// Positive while discharging.
+    pub current_ma: Option<i32>,
+    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<u8> = PowerBoardMap::new([11, 12, 13]);
+
+pub fn power_board(node_id: u8) -> Option<PowerBoardId> {
+    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),
         }
     }
 }

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<I, T, const N: usize> {
     values: [T; N],
@@ -96,6 +105,7 @@ pub type PressureSensorMap<T> = InventoryMap<PressSensId, T, 10>;
 pub type BinaryOutputMap<T> = InventoryMap<BinaryOutputId, T, 5>;
 pub type TankMap<T> = InventoryMap<TankId, T, 6>;
 pub type ServoMap<T> = InventoryMap<ServoId, T, 4>;
+pub type PowerBoardMap<T> = InventoryMap<PowerBoardId, T, 3>;
 
 /// 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::<BinaryOutputId, 5>();
         check::<TankId, 6>();
         check::<ServoId, 4>();
+        check::<PowerBoardId, 3>();
     }
 
     /// The two halves of the inventory travel in separate telemetry messages, so a tank whose