sitl: add closed-loop host flight runtime

tools/sitl/Cargo.toml

@@ -0,0 +1,17 @@
+[package]
+name = "kaptan-sitl"
+edition.workspace = true
+license.workspace = true
+repository.workspace = true
+rust-version.workspace = true
+version.workspace = true
+
+[lints]
+workspace = true
+
+[dependencies]
+kaptan-control = { path = "../../libs/kaptan-control" }
+kaptan-math = { path = "../../libs/kaptan-math" }
+kaptan-navigation = { path = "../../libs/kaptan-navigation", features = ["sitl"] }
+kaptan-safety = { path = "../../libs/kaptan-safety" }
+kaptan-time = { path = "../../libs/kaptan-time" }

tools/sitl/src/protocol.rs

@@ -0,0 +1,359 @@
+//! Fixed-size local UDP records for Kaptan's host-only simulator harness.
+//!
+//! This transport is deliberately not MAVLink and not part of Kaptan's flight
+//! dialect. It is confined to one host network namespace and is versioned so
+//! malformed simulator traffic never becomes estimator or motor authority.
+
+use std::net::SocketAddr;
+
+pub const SENSOR_PORT: u16 = 14_660;
+pub const ACTUATOR_PORT: u16 = 14_661;
+pub const CONTROL_PORT: u16 = 14_662;
+pub const VERSION: u8 = 1;
+pub const IMU_MAGIC: [u8; 4] = *b"KSI1";
+pub const LOCAL_STATE_MAGIC: [u8; 4] = *b"KSV1";
+/// Gazebo simulation time, sent once for every unpaused simulation step.
+/// This is the sole time base for host-SITL freshness and command leases.
+pub const CLOCK_MAGIC: [u8; 4] = *b"KSC1";
+pub const OUTPUT_MAGIC: [u8; 4] = *b"KSO1";
+pub const GOAL_MAGIC: [u8; 4] = *b"KSG1";
+/// A host-SITL-only request to descend to a declared local landing point and
+/// revoke motor authority once the landing point has settled.
+pub const LAND_MAGIC: [u8; 4] = *b"KSD1";
+pub const GOAL_KEEPALIVE_MAGIC: [u8; 4] = *b"KSK1";
+pub const STATUS_QUERY_MAGIC: [u8; 4] = *b"KSQ1";
+pub const STATUS_MAGIC: [u8; 4] = *b"KSS1";
+pub const GOAL_STATUS_MAGIC: [u8; 4] = *b"KSL1";
+
+pub const SENSOR_PACKET_SIZE: usize = 44;
+pub const CLOCK_PACKET_SIZE: usize = 16;
+pub const OUTPUT_PACKET_SIZE: usize = 40;
+pub const GOAL_PACKET_SIZE: usize = 24;
+pub const GOAL_KEEPALIVE_PACKET_SIZE: usize = 12;
+pub const STATUS_QUERY_PACKET_SIZE: usize = 8;
+pub const STATUS_PACKET_SIZE: usize = 20;
+pub const GOAL_STATUS_PACKET_SIZE: usize = 16;
+
+pub const STATUS_ESTIMATOR_READY: u16 = 1 << 0;
+pub const STATUS_IMU_FRESH: u16 = 1 << 1;
+pub const STATUS_LOCAL_AIDING_FRESH: u16 = 1 << 2;
+pub const STATUS_OUTPUT_PERMITTED: u16 = 1 << 3;
+pub const STATUS_ACTUATOR_TRANSPORT: u16 = 1 << 4;
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum PacketError {
+    Length,
+    Version,
+    NonFinite,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct ImuPacket {
+    pub time_microseconds: u64,
+    pub sequence: u32,
+    pub acceleration_frd_mps2: [f32; 3],
+    pub angular_rate_frd_rad_s: [f32; 3],
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct LocalStatePacket {
+    pub time_microseconds: u64,
+    pub sequence: u32,
+    pub position_ned_metres: [f32; 3],
+    pub velocity_ned_mps: [f32; 3],
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct ClockPacket {
+    pub time_microseconds: u64,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct GoalPacket {
+    pub goal_id: u32,
+    pub north_metres: f32,
+    pub east_metres: f32,
+    pub up_metres: f32,
+}
+
+/// Renews the command lease for one accepted goal without modifying it.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct GoalKeepalivePacket {
+    pub goal_id: u32,
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+#[repr(u8)]
+pub enum GoalLifecycle {
+    Rejected = 0,
+    Active = 1,
+    Settled = 2,
+    Landed = 3,
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct GoalStatusPacket {
+    pub goal_id: u32,
+    pub lifecycle: GoalLifecycle,
+}
+
+pub fn decode_imu(bytes: &[u8]) -> Result<ImuPacket, PacketError> {
+    let (time_microseconds, sequence, values) = decode_sensor(bytes, IMU_MAGIC)?;
+    Ok(ImuPacket {
+        time_microseconds,
+        sequence,
+        acceleration_frd_mps2: [values[0], values[1], values[2]],
+        angular_rate_frd_rad_s: [values[3], values[4], values[5]],
+    })
+}
+
+pub fn decode_local_state(bytes: &[u8]) -> Result<LocalStatePacket, PacketError> {
+    let (time_microseconds, sequence, values) = decode_sensor(bytes, LOCAL_STATE_MAGIC)?;
+    Ok(LocalStatePacket {
+        time_microseconds,
+        sequence,
+        position_ned_metres: [values[0], values[1], values[2]],
+        velocity_ned_mps: [values[3], values[4], values[5]],
+    })
+}
+
+pub fn decode_clock(bytes: &[u8]) -> Result<ClockPacket, PacketError> {
+    if bytes.len() != CLOCK_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != CLOCK_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    Ok(ClockPacket {
+        time_microseconds: read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
+    })
+}
+
+pub fn decode_goal(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
+    decode_goal_with_magic(bytes, GOAL_MAGIC, false)
+}
+
+/// Decodes a host-SITL landing request. Landing always targets the declared
+/// local ground plane (`up == 0`), rather than silently treating an arbitrary
+/// move goal as a landing operation.
+pub fn decode_landing(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
+    decode_goal_with_magic(bytes, LAND_MAGIC, true)
+}
+
+fn decode_goal_with_magic(
+    bytes: &[u8],
+    magic: [u8; 4],
+    require_ground_plane: bool,
+) -> Result<GoalPacket, PacketError> {
+    if bytes.len() != GOAL_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    let goal = GoalPacket {
+        goal_id: read_u32(&bytes[8..12]).ok_or(PacketError::Length)?,
+        north_metres: read_f32(&bytes[12..16]).ok_or(PacketError::Length)?,
+        east_metres: read_f32(&bytes[16..20]).ok_or(PacketError::Length)?,
+        up_metres: read_f32(&bytes[20..24]).ok_or(PacketError::Length)?,
+    };
+    if goal.goal_id == 0
+        || !goal.north_metres.is_finite()
+        || !goal.east_metres.is_finite()
+        || !goal.up_metres.is_finite()
+        || (require_ground_plane && goal.up_metres != 0.0)
+    {
+        return Err(PacketError::NonFinite);
+    }
+    Ok(goal)
+}
+
+pub fn decode_goal_keepalive(bytes: &[u8]) -> Result<GoalKeepalivePacket, PacketError> {
+    if bytes.len() != GOAL_KEEPALIVE_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != GOAL_KEEPALIVE_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    let goal_id = read_u32(&bytes[8..12]).ok_or(PacketError::Length)?;
+    if goal_id == 0 {
+        return Err(PacketError::NonFinite);
+    }
+    Ok(GoalKeepalivePacket { goal_id })
+}
+
+pub fn is_status_query(bytes: &[u8]) -> bool {
+    bytes.len() == STATUS_QUERY_PACKET_SIZE
+        && bytes[0..4] == STATUS_QUERY_MAGIC
+        && bytes[4] == VERSION
+        && bytes[5..8] == [0; 3]
+}
+
+pub fn encode_output(
+    time_microseconds: u64,
+    sequence: u32,
+    permitted: bool,
+    motors: [f32; 4],
+) -> Result<[u8; OUTPUT_PACKET_SIZE], PacketError> {
+    if motors
+        .iter()
+        .any(|motor| !motor.is_finite() || !(0.0..=1.0).contains(motor))
+        || (!permitted && motors.iter().any(|motor| *motor != 0.0))
+    {
+        return Err(PacketError::NonFinite);
+    }
+    let mut bytes = [0_u8; OUTPUT_PACKET_SIZE];
+    bytes[0..4].copy_from_slice(&OUTPUT_MAGIC);
+    bytes[4] = VERSION;
+    bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
+    bytes[16..20].copy_from_slice(&sequence.to_le_bytes());
+    bytes[20] = u8::from(permitted);
+    for (index, motor) in motors.iter().enumerate() {
+        bytes[24 + index * 4..28 + index * 4].copy_from_slice(&motor.to_le_bytes());
+    }
+    Ok(bytes)
+}
+
+pub fn encode_status(time_microseconds: u64, flags: u16) -> [u8; STATUS_PACKET_SIZE] {
+    let mut bytes = [0_u8; STATUS_PACKET_SIZE];
+    bytes[0..4].copy_from_slice(&STATUS_MAGIC);
+    bytes[4] = VERSION;
+    bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
+    bytes[16..18].copy_from_slice(&flags.to_le_bytes());
+    bytes
+}
+
+pub fn encode_goal_status(status: GoalStatusPacket) -> [u8; GOAL_STATUS_PACKET_SIZE] {
+    let mut bytes = [0_u8; GOAL_STATUS_PACKET_SIZE];
+    bytes[0..4].copy_from_slice(&GOAL_STATUS_MAGIC);
+    bytes[4] = VERSION;
+    bytes[8..12].copy_from_slice(&status.goal_id.to_le_bytes());
+    bytes[12] = status.lifecycle as u8;
+    bytes
+}
+
+pub fn localhost(sender: SocketAddr) -> bool {
+    sender.ip().is_loopback()
+}
+
+fn decode_sensor(bytes: &[u8], magic: [u8; 4]) -> Result<(u64, u32, [f32; 6]), PacketError> {
+    if bytes.len() != SENSOR_PACKET_SIZE {
+        return Err(PacketError::Length);
+    }
+    if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
+        return Err(PacketError::Version);
+    }
+    let mut values = [0.0; 6];
+    for (index, value) in values.iter_mut().enumerate() {
+        *value = read_f32(&bytes[20 + index * 4..24 + index * 4]).ok_or(PacketError::Length)?;
+    }
+    if values.iter().any(|value| !value.is_finite()) {
+        return Err(PacketError::NonFinite);
+    }
+    Ok((
+        read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
+        read_u32(&bytes[16..20]).ok_or(PacketError::Length)?,
+        values,
+    ))
+}
+
+fn read_u32(bytes: &[u8]) -> Option<u32> {
+    Some(u32::from_le_bytes(bytes.try_into().ok()?))
+}
+
+fn read_u64(bytes: &[u8]) -> Option<u64> {
+    Some(u64::from_le_bytes(bytes.try_into().ok()?))
+}
+
+fn read_f32(bytes: &[u8]) -> Option<f32> {
+    Some(f32::from_le_bytes(bytes.try_into().ok()?))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn sensor(magic: [u8; 4]) -> [u8; SENSOR_PACKET_SIZE] {
+        let mut bytes = [0_u8; SENSOR_PACKET_SIZE];
+        bytes[..4].copy_from_slice(&magic);
+        bytes[4] = VERSION;
+        bytes[8..16].copy_from_slice(&123_u64.to_le_bytes());
+        bytes[16..20].copy_from_slice(&9_u32.to_le_bytes());
+        for index in 0..6 {
+            bytes[20 + index * 4..24 + index * 4]
+                .copy_from_slice(&(index as f32 + 0.5).to_le_bytes());
+        }
+        bytes
+    }
+
+    #[test]
+    fn sensor_records_are_finite() {
+        let packet = decode_imu(&sensor(IMU_MAGIC)).unwrap();
+        assert_eq!(packet.time_microseconds, 123);
+        assert_eq!(packet.acceleration_frd_mps2, [0.5, 1.5, 2.5]);
+        assert_eq!(packet.angular_rate_frd_rad_s, [3.5, 4.5, 5.5]);
+        assert_eq!(
+            decode_imu(&sensor(LOCAL_STATE_MAGIC)),
+            Err(PacketError::Version)
+        );
+    }
+
+    #[test]
+    fn simulator_clock_has_no_host_receipt_time() {
+        let mut packet = [0_u8; CLOCK_PACKET_SIZE];
+        packet[..4].copy_from_slice(&CLOCK_MAGIC);
+        packet[4] = VERSION;
+        packet[8..16].copy_from_slice(&987_654_u64.to_le_bytes());
+        assert_eq!(
+            decode_clock(&packet),
+            Ok(ClockPacket {
+                time_microseconds: 987_654,
+            })
+        );
+        packet[4] = VERSION + 1;
+        assert_eq!(decode_clock(&packet), Err(PacketError::Version));
+    }
+
+    #[test]
+    fn inhibited_output() {
+        assert!(encode_output(1, 2, true, [0.1, 0.2, 0.3, 0.4]).is_ok());
+        assert_eq!(
+            encode_output(1, 2, false, [0.1, 0.0, 0.0, 0.0]),
+            Err(PacketError::NonFinite)
+        );
+    }
+
+    #[test]
+    fn goal_keepalive_validation() {
+        let mut packet = [0_u8; GOAL_KEEPALIVE_PACKET_SIZE];
+        packet[..4].copy_from_slice(&GOAL_KEEPALIVE_MAGIC);
+        packet[4] = VERSION;
+        packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
+        assert_eq!(
+            decode_goal_keepalive(&packet),
+            Ok(GoalKeepalivePacket { goal_id: 7 })
+        );
+        packet[8..12].copy_from_slice(&0_u32.to_le_bytes());
+        assert_eq!(decode_goal_keepalive(&packet), Err(PacketError::NonFinite));
+    }
+
+    #[test]
+    fn landing_is_an_explicit_ground_plane_command() {
+        let mut packet = [0_u8; GOAL_PACKET_SIZE];
+        packet[..4].copy_from_slice(&LAND_MAGIC);
+        packet[4] = VERSION;
+        packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
+        packet[12..16].copy_from_slice(&1.0_f32.to_le_bytes());
+        packet[16..20].copy_from_slice(&(-2.0_f32).to_le_bytes());
+        assert_eq!(
+            decode_landing(&packet),
+            Ok(GoalPacket {
+                goal_id: 7,
+                north_metres: 1.0,
+                east_metres: -2.0,
+                up_metres: 0.0,
+            })
+        );
+        packet[20..24].copy_from_slice(&0.1_f32.to_le_bytes());
+        assert_eq!(decode_landing(&packet), Err(PacketError::NonFinite));
+    }
+}