diff --git a/tools/sitl/Cargo.toml b/tools/sitl/Cargo.toml new file mode 100644 index 0000000..88a250b --- /dev/null +++ b/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" } diff --git a/tools/sitl/src/main.rs b/tools/sitl/src/main.rs new file mode 100644 index 0000000..0611b5b --- /dev/null +++ b/tools/sitl/src/main.rs @@ -0,0 +1,1309 @@ +//! Host-only closed-loop simulator executable. +//! +//! The process owns the same portable estimator and control primitives used by +//! Kaptan. Gazebo may supply simulated measurements, but only the ESKF nominal +//! state reaches the controller; simulator pose/velocity never has a parallel +//! controller path. + +mod protocol; + +use std::{ + env, + io::ErrorKind, + net::{SocketAddr, UdpSocket}, + process::ExitCode, + thread, + time::Duration, +}; + +use kaptan_control::{ + ActuatorLimits, Allocator, BodyRate, BodyWrench, HeadingNed, KinematicLimits, MixerOutput, + PidConfig, PrecomputedMixer, RatePid, ThrustEnvelope, Trajectory, TranslationalControlConfig, + TranslationalController, TranslationalEstimate, force_to_attitude, +}; +use kaptan_math::{Frd, Vector3}; +use kaptan_navigation::sitl::{ + EstimatorConfig, ImuNoise, ImuObservation, InertialEskf, IntegrityConfig, StationaryInitializer, +}; +use kaptan_navigation::{ + AccelerationFrd, AngularRateFrd, CalibrationRevision, FrameId, + LocalPositionVelocityObservation, ObservationContext, ObservationSequence, PositionNed, + ScalarVariance, SourceId, Validity, VelocityNed, +}; +use kaptan_safety::{ActuatorPermit, SafetyConfig, SafetyInputs, SafetySupervisor}; +use kaptan_time::Timestamp; + +use protocol::{ + ACTUATOR_PORT, CLOCK_MAGIC, CONTROL_PORT, GoalKeepalivePacket, GoalLifecycle, GoalPacket, + GoalStatusPacket, IMU_MAGIC, LOCAL_STATE_MAGIC, SENSOR_PORT, STATUS_ACTUATOR_TRANSPORT, + STATUS_ESTIMATOR_READY, STATUS_IMU_FRESH, STATUS_LOCAL_AIDING_FRESH, STATUS_OUTPUT_PERMITTED, +}; + +const SOURCE_IMU: SourceId = SourceId::new(1); +const SOURCE_LOCAL_STATE: SourceId = SourceId::new(2); +const CALIBRATION: CalibrationRevision = CalibrationRevision::new(1); +const IMU_FRESHNESS_MICROSECONDS: u64 = 50_000; +const LOCAL_STATE_FRESHNESS_MICROSECONDS: u64 = 150_000; +const COMMAND_LINK_FRESHNESS_MICROSECONDS: u64 = 500_000; +/// Host-simulation policy only. Flight targets must supply a reviewed vehicle +/// profile rather than reuse this value. +const SITL_PREARM_DWELL_MICROSECONDS: u64 = 100_000; +/// Bound either ingress lane so a fast simulator clock cannot indefinitely +/// delay command lease processing, and a command flood cannot delay sensor +/// ordering and safety supervision. +const MAXIMUM_INGRESS_DATAGRAMS_PER_TURN: usize = 32; +const MAXIMUM_IMU_INTERVAL_MICROSECONDS: u64 = 50_000; +// TODO(configuration): bind these values, vehicle mass/inertia, mixer, and +// flight limits to the reviewed vehicle profile once the ground configuration +// protocol exists. They are intentionally confined to this host-only model. +const POSITION_VARIANCE_M2: f32 = 0.04; +const VELOCITY_VARIANCE_M2PS2: f32 = 0.04; + +fn main() -> ExitCode { + match env::args().nth(1).as_deref() { + Some("serve") => match serve() { + Ok(()) => ExitCode::SUCCESS, + Err(error) => { + eprintln!("kaptan-sitl: {error}"); + ExitCode::FAILURE + } + }, + _ => { + eprintln!("usage: kaptan-sitl serve"); + ExitCode::FAILURE + } + } +} + +fn serve() -> Result<(), String> { + let sensor = bind_loopback(SENSOR_PORT)?; + let control = bind_loopback(CONTROL_PORT)?; + let actuator = UdpSocket::bind(("127.0.0.1", 0)).map_err(|error| error.to_string())?; + sensor + .set_nonblocking(true) + .map_err(|error| error.to_string())?; + control + .set_nonblocking(true) + .map_err(|error| error.to_string())?; + let mut runtime = SitlRuntime::new()?; + eprintln!( + "kaptan-sitl: listening for Gazebo sensors on 127.0.0.1:{SENSOR_PORT} and commands on 127.0.0.1:{CONTROL_PORT}" + ); + + loop { + let mut made_progress = false; + for _ in 0..MAXIMUM_INGRESS_DATAGRAMS_PER_TURN { + let Some((packet, sender)) = receive(&sensor)? else { + break; + }; + made_progress = true; + if !protocol::localhost(sender) { + continue; + } + runtime.accept_sensor(&packet, &actuator, &control)?; + } + for _ in 0..MAXIMUM_INGRESS_DATAGRAMS_PER_TURN { + let Some((packet, sender)) = receive(&control)? else { + break; + }; + made_progress = true; + if !protocol::localhost(sender) { + continue; + } + runtime.accept_control(&packet, sender, &control)?; + } + if !made_progress { + // Freshness, lease expiry, and safety supervision advance only + // when Gazebo sends an explicit simulation clock packet. Host + // scheduling must not change the simulated vehicle behavior. + thread::sleep(Duration::from_millis(1)); + } + } +} + +fn bind_loopback(port: u16) -> Result { + UdpSocket::bind(("127.0.0.1", port)).map_err(|error| { + format!("cannot bind 127.0.0.1:{port}; another simulator may still be running: {error}") + }) +} + +fn receive(socket: &UdpSocket) -> Result, SocketAddr)>, String> { + let mut buffer = [0_u8; 256]; + match socket.recv_from(&mut buffer) { + Ok((length, sender)) => Ok(Some((buffer[..length].to_vec(), sender))), + Err(error) if error.kind() == ErrorKind::WouldBlock => Ok(None), + Err(error) => Err(error.to_string()), + } +} + +/// Private host-SITL datagram boundary. Production uses only loopback UDP; +/// the trait lets unit tests observe the exact encoded records without opening +/// a host socket or introducing another actuator path. +trait DatagramSink { + fn send_datagram(&self, bytes: &[u8], destination: SocketAddr) -> Result<(), String>; +} + +impl DatagramSink for UdpSocket { + fn send_datagram(&self, bytes: &[u8], destination: SocketAddr) -> Result<(), String> { + self.send_to(bytes, destination) + .map(|_| ()) + .map_err(|error| error.to_string()) + } +} + +struct SitlRuntime { + initializer: StationaryInitializer<100>, + estimator: Option, + controller: QuadController, + trajectory: Option, + safety: SafetySupervisor, + active_goal: Option, + landing_goal: bool, + goal_settled: bool, + goal_client: Option, + output_sequence: u32, + simulation_time: Option, + last_imu_time: Option, + last_imu_sequence: Option, + last_local_state_time: Option, + last_local_state_sequence: Option, + last_command_time: Option, + pending_control_step: Option<(u64, Option)>, + last_rate: BodyRate, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +enum ClockWork { + Ignore, + Supervise, + Control(Option), +} + +impl SitlRuntime { + fn new() -> Result { + Ok(Self { + initializer: StationaryInitializer::new(), + estimator: None, + controller: QuadController::new()?, + trajectory: None, + safety: SafetySupervisor::new( + SafetyConfig::new(SITL_PREARM_DWELL_MICROSECONDS) + .map_err(|error| format!("invalid SITL safety policy: {error:?}"))?, + ), + active_goal: None, + landing_goal: false, + goal_settled: false, + goal_client: None, + output_sequence: 0, + simulation_time: None, + last_imu_time: None, + last_imu_sequence: None, + last_local_state_time: None, + last_local_state_sequence: None, + last_command_time: None, + pending_control_step: None, + last_rate: BodyRate::radians_per_second(0.0, 0.0, 0.0), + }) + } + + fn accept_sensor( + &mut self, + bytes: &[u8], + actuator: &A, + control: &C, + ) -> Result<(), String> { + if bytes.len() >= 4 && bytes[..4] == IMU_MAGIC { + if let Ok(packet) = protocol::decode_imu(bytes) { + self.accept_imu(packet, actuator)?; + } + } else if bytes.len() >= 4 + && bytes[..4] == LOCAL_STATE_MAGIC + && let Ok(packet) = protocol::decode_local_state(bytes) + { + self.accept_local_state(packet); + } else if bytes.len() >= 4 + && bytes[..4] == CLOCK_MAGIC + && let Ok(packet) = protocol::decode_clock(bytes) + { + self.accept_clock(packet, actuator, control)?; + } + Ok(()) + } + + fn accept_control( + &mut self, + bytes: &[u8], + sender: SocketAddr, + socket: &C, + ) -> Result<(), String> { + if let Some(now) = self.simulation_time { + self.expire_stale_goal(now); + } + if protocol::is_status_query(bytes) { + socket.send_datagram( + &protocol::encode_status(self.current_time(), self.status_flags()), + sender, + )?; + return Ok(()); + } + if let Ok(keepalive) = protocol::decode_goal_keepalive(bytes) { + self.renew_goal(keepalive, sender); + return Ok(()); + } + if let Ok(goal) = protocol::decode_landing(bytes) { + return self.start_goal(goal, true, sender, socket); + } + let Ok(goal) = protocol::decode_goal(bytes) else { + return Ok(()); + }; + self.start_goal(goal, false, sender, socket) + } + + fn start_goal( + &mut self, + goal: GoalPacket, + landing: bool, + sender: SocketAddr, + socket: &C, + ) -> Result<(), String> { + let Some(now) = self.simulation_time else { + return self.send_goal_status_to( + socket, + sender, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle: GoalLifecycle::Rejected, + }, + ); + }; + if let Some(active) = self.active_goal { + if self.goal_client != Some(sender) { + return self.send_goal_status_to( + socket, + sender, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle: GoalLifecycle::Rejected, + }, + ); + } + if active == goal && self.landing_goal == landing { + self.last_command_time = self.simulation_time; + return self.send_goal_status_to( + socket, + sender, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle: if self.goal_settled { + GoalLifecycle::Settled + } else { + GoalLifecycle::Active + }, + }, + ); + } + if active.goal_id == goal.goal_id { + return self.send_goal_status_to( + socket, + sender, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle: GoalLifecycle::Rejected, + }, + ); + } + } + let Some(estimator) = self.estimator else { + return self.send_goal_status_to( + socket, + sender, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle: GoalLifecycle::Rejected, + }, + ); + }; + let health = estimator.health(estimator.time()); + if !health.initialized + || !health.finite + || !health.capabilities.local_position + || !health.capabilities.local_velocity + { + return self.send_goal_status_to( + socket, + sender, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle: GoalLifecycle::Rejected, + }, + ); + } + let initial_position = estimator.state().position().vector(); + let target = Vector3::new(goal.north_metres, goal.east_metres, -goal.up_metres); + let limits = flight_limits(); + match self.trajectory.as_mut() { + Some(trajectory) => trajectory + .set_goal(target) + .map_err(|error| format!("invalid goal: {error:?}"))?, + None => { + let mut trajectory = Trajectory::new(initial_position, limits) + .map_err(|error| format!("invalid trajectory configuration: {error:?}"))?; + trajectory + .set_goal(target) + .map_err(|error| format!("invalid goal: {error:?}"))?; + self.trajectory = Some(trajectory); + } + } + self.active_goal = Some(goal); + self.landing_goal = landing; + self.goal_settled = false; + self.goal_client = Some(sender); + self.last_command_time = Some(now); + let _ = self + .safety + .request_arm(Timestamp::from_microseconds(now), self.safety_inputs(now)); + self.send_goal_status_to( + socket, + sender, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle: GoalLifecycle::Active, + }, + ) + } + + fn renew_goal(&mut self, keepalive: GoalKeepalivePacket, sender: SocketAddr) { + if self.goal_client == Some(sender) + && self + .active_goal + .is_some_and(|goal| goal.goal_id == keepalive.goal_id) + { + self.last_command_time = self.simulation_time; + } + } + + fn accept_imu( + &mut self, + packet: protocol::ImuPacket, + actuator: &A, + ) -> Result<(), String> { + let reset_epoch = !strictly_after( + packet.time_microseconds, + packet.sequence, + self.last_imu_time, + self.last_imu_sequence, + ); + if reset_epoch { + self.reset_for_simulator_epoch(); + } + let step_seconds = if reset_epoch { + None + } else { + control_step_seconds(self.last_imu_time, packet.time_microseconds) + }; + if self.estimator.is_some() && self.last_imu_time.is_some() && step_seconds.is_none() { + self.reset_for_simulator_epoch(); + } + self.last_imu_time = Some(packet.time_microseconds); + self.last_imu_sequence = Some(packet.sequence); + self.last_rate = BodyRate::radians_per_second( + packet.angular_rate_frd_rad_s[0], + packet.angular_rate_frd_rad_s[1], + packet.angular_rate_frd_rad_s[2], + ); + let observation = imu_observation(packet)?; + if let Some(estimator) = self.estimator.as_mut() { + if estimator.propagate(observation).is_err() { + self.reset_for_simulator_epoch(); + self.send_inhibited_output(actuator, packet.time_microseconds)?; + return Ok(()); + } + } else if self.initializer.push(observation) && self.initializer.is_complete() { + self.estimator = core::mem::take(&mut self.initializer).finish( + observation.context.measurement_time, + estimator_config(), + IntegrityConfig::default(), + 0.0, + ); + } + self.pending_control_step = Some((packet.time_microseconds, step_seconds)); + // An invalid propagation already sent a zero command above. Control + // advances only when the matching explicit simulation clock arrives. + Ok(()) + } + + fn accept_local_state(&mut self, packet: protocol::LocalStatePacket) { + if !strictly_after( + packet.time_microseconds, + packet.sequence, + self.last_local_state_time, + self.last_local_state_sequence, + ) { + return; + } + self.last_local_state_time = Some(packet.time_microseconds); + self.last_local_state_sequence = Some(packet.sequence); + let Some(estimator) = self.estimator.as_mut() else { + return; + }; + let Some(position_variance) = ScalarVariance::new(POSITION_VARIANCE_M2) else { + return; + }; + let Some(velocity_variance) = ScalarVariance::new(VELOCITY_VARIANCE_M2PS2) else { + return; + }; + let observation = LocalPositionVelocityObservation { + context: ObservationContext::new( + Timestamp::from_microseconds(packet.time_microseconds), + Timestamp::from_microseconds(packet.time_microseconds), + SOURCE_LOCAL_STATE, + FrameId::LOCAL_NED, + CALIBRATION, + u8::MAX, + Validity::Valid, + ObservationSequence::new(packet.sequence), + ), + position: PositionNed::metres( + packet.position_ned_metres[0], + packet.position_ned_metres[1], + packet.position_ned_metres[2], + ), + velocity: VelocityNed::metres_per_second( + packet.velocity_ned_mps[0], + packet.velocity_ned_mps[1], + packet.velocity_ned_mps[2], + ), + position_variance, + velocity_variance, + }; + let _ = + estimator.fuse_local_position_velocity(observation, SOURCE_LOCAL_STATE, CALIBRATION); + } + + /// Advances all non-estimator supervision from Gazebo's monotonic + /// simulation clock. IMU packets schedule control, but they cannot advance + /// leases or freshness themselves: the matching clock record is required. + fn accept_clock( + &mut self, + packet: protocol::ClockPacket, + actuator: &A, + control: &C, + ) -> Result<(), String> { + match self.advance_simulation_clock(packet.time_microseconds) { + ClockWork::Ignore => Ok(()), + ClockWork::Control(step_seconds) => { + self.control(packet.time_microseconds, step_seconds, actuator, control) + } + ClockWork::Supervise => self.supervise_clock(packet.time_microseconds, actuator), + } + } + + fn advance_simulation_clock(&mut self, simulation_time: u64) -> ClockWork { + match self.simulation_time { + Some(previous) if simulation_time == previous => return ClockWork::Ignore, + Some(previous) if simulation_time < previous => { + self.reset_for_simulator_epoch(); + } + _ => {} + } + self.simulation_time = Some(simulation_time); + + match self.pending_control_step { + Some((imu_time, step_seconds)) if imu_time == simulation_time => { + self.pending_control_step = None; + ClockWork::Control(step_seconds) + } + Some((imu_time, _)) if imu_time < simulation_time => { + // The required clock tick was lost or reordered. Never use a + // stale measurement interval to advance the controller. + self.pending_control_step = None; + ClockWork::Supervise + } + _ => ClockWork::Supervise, + } + } + + fn control( + &mut self, + simulation_time: u64, + step_seconds: Option, + actuator: &A, + control: &C, + ) -> Result<(), String> { + self.expire_stale_goal(simulation_time); + let inputs = self.safety_inputs(simulation_time); + let decision = self + .safety + .evaluate(Timestamp::from_microseconds(simulation_time), inputs); + let permitted = match decision.permit() { + Some(permit) if self.safety.permit_is_current(permit) => { + let motors = match (self.estimator, self.trajectory.as_mut(), step_seconds) { + (Some(estimator), Some(trajectory), Some(dt_seconds)) => trajectory + .update(dt_seconds) + .map_err(|_| ()) + .and_then(|reference| { + self.controller + .update(estimator, reference, self.last_rate, dt_seconds) + .map_err(|_| ()) + }), + _ => Ok([0.0; 4]), + }; + match motors { + Ok(motors) => { + self.send_armed_output(actuator, simulation_time, permit, motors)? + } + Err(()) => { + self.abort_goal(); + self.send_inhibited_output(actuator, simulation_time)?; + false + } + } + } + _ => { + self.controller.reset(); + self.send_inhibited_output(actuator, simulation_time)?; + false + } + }; + if permitted + && !self.goal_settled + && self.goal_is_settled() + && let Some(goal) = self.active_goal + { + let lifecycle = if self.landing_goal { + GoalLifecycle::Landed + } else { + GoalLifecycle::Settled + }; + self.send_goal_status( + control, + GoalStatusPacket { + goal_id: goal.goal_id, + lifecycle, + }, + )?; + self.goal_settled = true; + if self.landing_goal { + // Landing is terminal. Revoke authority in the same IMU-timed + // control cycle that reports it; a client cannot keep motors + // active merely by renewing the preceding trajectory lease. + self.abort_goal(); + self.send_inhibited_output(actuator, simulation_time)?; + } + } + Ok(()) + } + + /// Evaluates freshness on every explicit simulation clock tick, including + /// ticks where an IMU sample was absent. Wall-clock idling does nothing. + fn supervise_clock( + &mut self, + simulation_time: u64, + actuator: &A, + ) -> Result<(), String> { + self.expire_stale_goal(simulation_time); + let inputs = self.safety_inputs(simulation_time); + let decision = self + .safety + .evaluate(Timestamp::from_microseconds(simulation_time), inputs); + if decision.permit().is_none() { + self.controller.reset(); + self.send_inhibited_output(actuator, simulation_time)?; + } + Ok(()) + } + + fn safety_inputs(&self, now: u64) -> SafetyInputs { + let estimator_healthy = self.estimator.is_some_and(|estimator| { + let health = estimator.health(estimator.time()); + health.initialized + && health.finite + && health.capabilities.attitude + && health.capabilities.local_position + && health.capabilities.local_velocity + }) && fresh( + self.last_local_state_time, + now, + LOCAL_STATE_FRESHNESS_MICROSECONDS, + ); + SafetyInputs { + estimator_healthy, + imu_fresh: fresh(self.last_imu_time, now, IMU_FRESHNESS_MICROSECONDS), + link_fresh: fresh( + self.last_command_time, + now, + COMMAND_LINK_FRESHNESS_MICROSECONDS, + ), + actuator_healthy: true, + } + } + + fn expire_stale_goal(&mut self, now: u64) { + if self.active_goal.is_some() + && !fresh( + self.last_command_time, + now, + COMMAND_LINK_FRESHNESS_MICROSECONDS, + ) + { + self.abort_goal(); + } + } + + fn abort_goal(&mut self) { + self.safety.request_disarm(); + self.trajectory = None; + self.active_goal = None; + self.landing_goal = false; + self.goal_settled = false; + self.goal_client = None; + self.last_command_time = None; + self.controller.reset(); + } + + /// Revalidates the supervisor token at the sole nonzero-output boundary. + fn send_armed_output( + &mut self, + actuator: &A, + simulation_time: u64, + permit: &ActuatorPermit, + motors: [f32; 4], + ) -> Result { + if !self.safety.permit_is_current(permit) { + self.send_inhibited_output(actuator, simulation_time)?; + return Ok(false); + } + self.send_wire_output(actuator, simulation_time, true, motors)?; + Ok(true) + } + + /// Sends an explicit zero, inhibited output without requiring a permit. + fn send_inhibited_output( + &mut self, + actuator: &A, + simulation_time: u64, + ) -> Result<(), String> { + self.send_wire_output(actuator, simulation_time, false, [0.0; 4]) + } + + fn send_wire_output( + &mut self, + actuator: &A, + simulation_time: u64, + permitted: bool, + motors: [f32; 4], + ) -> Result<(), String> { + let output = + protocol::encode_output(simulation_time, self.output_sequence, permitted, motors) + .map_err(|error| format!("refusing invalid motor output: {error:?}"))?; + self.output_sequence = self.output_sequence.wrapping_add(1); + actuator.send_datagram(&output, SocketAddr::from(([127, 0, 0, 1], ACTUATOR_PORT))) + } + + fn status_flags(&self) -> u16 { + let now = self.current_time(); + let mut flags = STATUS_ACTUATOR_TRANSPORT; + if fresh(self.last_imu_time, now, IMU_FRESHNESS_MICROSECONDS) { + flags |= STATUS_IMU_FRESH; + } + if fresh( + self.last_local_state_time, + now, + LOCAL_STATE_FRESHNESS_MICROSECONDS, + ) { + flags |= STATUS_LOCAL_AIDING_FRESH; + } + if self.estimator.is_some_and(|estimator| { + let health = estimator.health(estimator.time()); + health.initialized + && health.finite + && health.capabilities.local_position + && health.capabilities.local_velocity + }) { + flags |= STATUS_ESTIMATOR_READY; + } + if self.safety.state() == kaptan_safety::SafetyState::Armed { + flags |= STATUS_OUTPUT_PERMITTED; + } + flags + } + + fn current_time(&self) -> u64 { + self.simulation_time.unwrap_or(0) + } + + fn send_goal_status( + &self, + socket: &C, + status: GoalStatusPacket, + ) -> Result<(), String> { + let Some(client) = self.goal_client else { + return Ok(()); + }; + self.send_goal_status_to(socket, client, status) + } + + fn send_goal_status_to( + &self, + socket: &C, + client: SocketAddr, + status: GoalStatusPacket, + ) -> Result<(), String> { + socket.send_datagram(&protocol::encode_goal_status(status), client) + } + + fn reset_for_simulator_epoch(&mut self) { + self.initializer = StationaryInitializer::new(); + self.estimator = None; + self.abort_goal(); + self.safety.reset_time_epoch(); + self.simulation_time = None; + self.last_imu_time = None; + self.last_imu_sequence = None; + self.last_local_state_time = None; + self.last_local_state_sequence = None; + self.last_command_time = None; + self.pending_control_step = None; + self.last_rate = BodyRate::radians_per_second(0.0, 0.0, 0.0); + self.controller.reset(); + } + + fn goal_is_settled(&self) -> bool { + let (Some(estimator), Some(trajectory)) = (self.estimator, self.trajectory.as_ref()) else { + return false; + }; + if !trajectory.is_complete(0.05, 0.05) { + return false; + } + let state = estimator.state(); + let error = trajectory.goal().sub(state.position().vector()); + error.norm_squared() <= 0.25 * 0.25 + && state.velocity().vector().norm_squared() <= 0.25 * 0.25 + } +} + +struct QuadController { + translational: TranslationalController, + thrust: ThrustEnvelope, + rate: RatePid, + allocator: Allocator<4>, +} + +impl QuadController { + fn new() -> Result { + // TODO(configuration): replace this one test vehicle's validated + // constants with an immutable, reviewed configuration selected before + // arming. No board target may inherit this host-SITL profile. + let translational = TranslationalController::new(TranslationalControlConfig { + position_gain_per_second_squared: 0.8, + velocity_gain_per_second: 1.2, + maximum_horizontal_acceleration_mps2: 3.0, + maximum_upward_acceleration_mps2: 2.0, + maximum_downward_acceleration_mps2: 2.0, + }) + .map_err(|error| format!("invalid translational controller: {error:?}"))?; + let thrust = ThrustEnvelope { + mass_kg: 1.2, + maximum_collective_thrust_newtons: 18.0, + // tan(35 degrees), computed offline so the embedded rate path + // requires no transcendental implementation. + maximum_tilt_tangent: 0.700_207_53, + }; + thrust + .validate() + .map_err(|error| format!("invalid thrust envelope: {error:?}"))?; + let rate_config = PidConfig { + proportional: 0.035, + integral: 0.006, + derivative: 0.0, + output_limit: 0.10, + integral_limit: 0.025, + }; + let limits = ActuatorLimits { + minimum: 0.0, + maximum: 1.0, + slew_per_second: 8.0, + }; + let mixer = PrecomputedMixer::new([ + MixerOutput { + coefficients: [-1.0, 1.0, 1.0, 1.0], + limits, + }, + MixerOutput { + coefficients: [-1.0, -1.0, -1.0, 1.0], + limits, + }, + MixerOutput { + coefficients: [1.0, -1.0, 1.0, 1.0], + limits, + }, + MixerOutput { + coefficients: [1.0, 1.0, -1.0, 1.0], + limits, + }, + ]); + let allocator = + Allocator::new(mixer).map_err(|error| format!("invalid mixer: {error:?}"))?; + Ok(Self { + translational, + thrust, + rate: RatePid::new(rate_config, rate_config, rate_config), + allocator, + }) + } + + fn update( + &mut self, + estimator: InertialEskf, + reference: kaptan_control::KinematicReference, + measured_rate: BodyRate, + dt_seconds: f32, + ) -> Result<[f32; 4], String> { + let state = estimator.state(); + let estimate = TranslationalEstimate { + position: state.position().vector(), + velocity: state.velocity().vector(), + }; + let acceleration = self + .translational + .update(reference, estimate) + .map_err(|error| format!("translational control rejected estimate: {error:?}"))?; + let (force, force_limited) = self + .thrust + .force_for_acceleration(acceleration, 9.806_65) + .map_err(|error| format!("thrust envelope rejected command: {error:?}"))?; + let heading_vector = state + .attitude() + .body_to_ned(Vector3::::new(1.0, 0.0, 0.0)); + let heading = HeadingNed::new(heading_vector.x(), heading_vector.y()) + .ok_or_else(|| "estimator produced a vertical/invalid heading".to_owned())?; + let target_attitude = force_to_attitude(force, heading) + .map_err(|error| format!("force-to-attitude failed: {error:?}"))?; + let error = state + .attitude() + .inverse() + .compose(target_attitude) + .map_err(|error| format!("attitude composition failed: {error:?}"))?; + let direction = if error.w() < 0.0 { -1.0 } else { 1.0 }; + let target_rate = BodyRate::radians_per_second( + direction * 2.5 * error.x(), + direction * 2.5 * error.y(), + direction * 1.0 * error.z(), + ); + let torque = self + .rate + .update(target_rate, measured_rate, dt_seconds) + .map_err(|error| format!("rate controller failed: {error:?}"))?; + let collective = force.vector().norm() / self.thrust.maximum_collective_thrust_newtons; + let allocation = self + .allocator + .allocate( + BodyWrench { + roll: torque.x(), + pitch: torque.y(), + yaw: torque.z(), + collective, + }, + dt_seconds, + ) + .map_err(|error| format!("mixer failed: {error:?}"))?; + if force_limited || allocation.saturated { + self.rate.reset_integrators(); + } + Ok(allocation.commands) + } + + fn reset(&mut self) { + self.rate.reset(); + self.allocator.reset_commands(); + } +} + +fn imu_observation(packet: protocol::ImuPacket) -> Result { + let Some(gyro_rate) = ScalarVariance::new(2.5e-5) else { + return Err("invalid static gyro variance".to_owned()); + }; + let Some(acceleration) = ScalarVariance::new(2.5e-3) else { + return Err("invalid static acceleration variance".to_owned()); + }; + Ok(ImuObservation { + context: ObservationContext::new( + Timestamp::from_microseconds(packet.time_microseconds), + Timestamp::from_microseconds(packet.time_microseconds), + SOURCE_IMU, + FrameId::BODY_FRD, + CALIBRATION, + u8::MAX, + Validity::Valid, + ObservationSequence::new(packet.sequence), + ), + acceleration: AccelerationFrd::metres_per_second_squared( + packet.acceleration_frd_mps2[0], + packet.acceleration_frd_mps2[1], + packet.acceleration_frd_mps2[2], + ), + angular_rate: AngularRateFrd::radians_per_second( + packet.angular_rate_frd_rad_s[0], + packet.angular_rate_frd_rad_s[1], + packet.angular_rate_frd_rad_s[2], + ), + noise: ImuNoise { + gyro_rate, + acceleration, + }, + }) +} + +fn estimator_config() -> EstimatorConfig { + EstimatorConfig { + maximum_imu_interval_microseconds: MAXIMUM_IMU_INTERVAL_MICROSECONDS, + ..EstimatorConfig::default() + } +} + +fn control_step_seconds(previous: Option, current: u64) -> Option { + let elapsed = current.checked_sub(previous?)?; + if elapsed == 0 || elapsed > MAXIMUM_IMU_INTERVAL_MICROSECONDS { + return None; + } + Some(elapsed as f32 / 1_000_000.0) +} + +fn flight_limits() -> KinematicLimits { + KinematicLimits { + maximum_horizontal_speed_mps: 3.0, + maximum_vertical_speed_mps: 1.5, + maximum_horizontal_acceleration_mps2: 2.0, + maximum_vertical_acceleration_mps2: 1.5, + } +} + +fn fresh(measurement_time: Option, now: u64, limit_microseconds: u64) -> bool { + measurement_time + .and_then(|measurement_time| now.checked_sub(measurement_time)) + .is_some_and(|age| age <= limit_microseconds) +} + +fn strictly_after( + time: u64, + sequence: u32, + previous_time: Option, + previous_sequence: Option, +) -> bool { + match (previous_time, previous_sequence) { + (Some(previous_time), Some(previous_sequence)) => { + time > previous_time + && sequence.wrapping_sub(previous_sequence) != 0 + && sequence.wrapping_sub(previous_sequence) <= u32::MAX / 2 + } + _ => true, + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::cell::RefCell; + + #[derive(Default)] + struct RecordingSink { + records: RefCell, SocketAddr)>>, + } + + impl RecordingSink { + fn records(&self) -> Vec<(Vec, SocketAddr)> { + self.records.borrow().clone() + } + } + + impl DatagramSink for RecordingSink { + fn send_datagram(&self, bytes: &[u8], destination: SocketAddr) -> Result<(), String> { + self.records + .borrow_mut() + .push((bytes.to_vec(), destination)); + Ok(()) + } + } + + fn initialized_estimator() -> InertialEskf { + let initializer = StationaryInitializer::<4>::new(); + let imu = imu_observation(protocol::ImuPacket { + time_microseconds: 10, + sequence: 1, + acceleration_frd_mps2: [0.0, 0.0, -9.806_65], + angular_rate_frd_rad_s: [0.0; 3], + }) + .unwrap(); + let mut initializer = initializer; + for sequence in 1..=4 { + initializer.push(ImuObservation { + context: ObservationContext::new( + Timestamp::from_microseconds(sequence * 10_000), + Timestamp::from_microseconds(sequence * 10_000), + SOURCE_IMU, + FrameId::BODY_FRD, + CALIBRATION, + u8::MAX, + Validity::Valid, + ObservationSequence::new(sequence as u32), + ), + ..imu + }); + } + initializer + .finish( + Timestamp::from_microseconds(40_000), + estimator_config(), + IntegrityConfig::default(), + 0.0, + ) + .unwrap() + } + + #[test] + fn sequence_order() { + assert!(strictly_after(2, 0, Some(1), Some(u32::MAX))); + assert!(!strictly_after(2, 7, Some(2), Some(6))); + assert!(!strictly_after(1, 8, Some(2), Some(7))); + } + + #[test] + fn hover_output() { + let mut controller = QuadController::new().unwrap(); + let filter = initialized_estimator(); + let outputs = controller + .update( + filter, + kaptan_control::KinematicReference { + position: Vector3::zero(), + velocity: Vector3::zero(), + acceleration: Vector3::zero(), + }, + BodyRate::radians_per_second(0.0, 0.0, 0.0), + 0.01, + ) + .unwrap(); + assert!( + outputs + .iter() + .all(|output| *output >= 0.0 && *output <= 1.0) + ); + } + + #[test] + fn settled_landing_reports_landed_then_emits_inhibited_zero_output() { + let now = SITL_PREARM_DWELL_MICROSECONDS; + let mut runtime = SitlRuntime::new().unwrap(); + runtime.estimator = Some(initialized_estimator()); + runtime.accept_local_state(protocol::LocalStatePacket { + time_microseconds: 40_000, + sequence: 1, + position_ned_metres: [0.0; 3], + velocity_ned_mps: [0.0; 3], + }); + runtime.simulation_time = Some(now); + runtime.last_imu_time = Some(now); + runtime.last_local_state_time = Some(now); + runtime.last_command_time = Some(now); + runtime.active_goal = Some(GoalPacket { + goal_id: 17, + north_metres: 0.0, + east_metres: 0.0, + up_metres: 0.0, + }); + runtime.landing_goal = true; + runtime.goal_client = Some(SocketAddr::from(([127, 0, 0, 1], 15_001))); + runtime.trajectory = Some(Trajectory::new(Vector3::zero(), flight_limits()).unwrap()); + let healthy = runtime.safety_inputs(now); + runtime + .safety + .request_arm(Timestamp::from_microseconds(0), healthy); + assert_eq!( + runtime + .safety + .evaluate(Timestamp::from_microseconds(now), healthy) + .state(), + kaptan_safety::SafetyState::Armed + ); + + let datagrams = RecordingSink::default(); + runtime + .control(now, Some(0.01), &datagrams, &datagrams) + .unwrap(); + + let records = datagrams.records(); + assert_eq!(records.len(), 3); + assert_eq!( + records[0].1, + SocketAddr::from(([127, 0, 0, 1], ACTUATOR_PORT)) + ); + assert_eq!(&records[0].0[..4], &protocol::OUTPUT_MAGIC); + assert_eq!(records[0].0[20], 1); + assert_eq!(records[1].1, SocketAddr::from(([127, 0, 0, 1], 15_001))); + assert_eq!(&records[1].0[..4], &protocol::GOAL_STATUS_MAGIC); + assert_eq!( + u32::from_le_bytes(records[1].0[8..12].try_into().unwrap()), + 17 + ); + assert_eq!(records[1].0[12], GoalLifecycle::Landed as u8); + assert_eq!( + records[2].1, + SocketAddr::from(([127, 0, 0, 1], ACTUATOR_PORT)) + ); + assert_eq!(&records[2].0[..4], &protocol::OUTPUT_MAGIC); + assert_eq!(records[2].0[20], 0); + assert_eq!(&records[2].0[24..], &[0_u8; 16]); + assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); + assert!(runtime.active_goal.is_none()); + assert!(runtime.trajectory.is_none()); + } + + #[test] + fn control_step_tracks_imu_time() { + assert!(control_step_seconds(None, 10_000).is_none()); + assert!(control_step_seconds(Some(10_000), 10_000).is_none()); + assert!(control_step_seconds(Some(10_000), 9_999).is_none()); + assert!(control_step_seconds(Some(10_000), 60_001).is_none()); + assert!((control_step_seconds(Some(10_000), 20_000).unwrap() - 0.01).abs() < f32::EPSILON); + assert!((control_step_seconds(Some(10_000), 60_000).unwrap() - 0.05).abs() < f32::EPSILON); + } + + #[test] + fn lease_expiry_clears_authority() { + let mut runtime = SitlRuntime::new().unwrap(); + runtime.simulation_time = Some(1_000_000); + let client = SocketAddr::from(([127, 0, 0, 1], 15_001)); + runtime.active_goal = Some(GoalPacket { + goal_id: 7, + north_metres: 1.0, + east_metres: 2.0, + up_metres: 3.0, + }); + runtime.goal_client = Some(client); + runtime.goal_settled = true; + runtime.trajectory = Some(Trajectory::new(Vector3::zero(), flight_limits()).unwrap()); + runtime.last_command_time = Some(1_000_000); + runtime.safety.request_arm( + Timestamp::from_microseconds(0), + SafetyInputs { + estimator_healthy: true, + imu_fresh: true, + link_fresh: true, + actuator_healthy: true, + }, + ); + let healthy = SafetyInputs { + estimator_healthy: true, + imu_fresh: true, + link_fresh: true, + actuator_healthy: true, + }; + runtime.safety.evaluate( + Timestamp::from_microseconds(SITL_PREARM_DWELL_MICROSECONDS), + healthy, + ); + + runtime.expire_stale_goal(1_000_000 + COMMAND_LINK_FRESHNESS_MICROSECONDS); + assert!(runtime.active_goal.is_some()); + + runtime.expire_stale_goal(1_000_001 + COMMAND_LINK_FRESHNESS_MICROSECONDS); + + assert!(runtime.active_goal.is_none()); + assert!(runtime.goal_client.is_none()); + assert!(!runtime.goal_settled); + assert!(runtime.trajectory.is_none()); + assert!(runtime.last_command_time.is_none()); + assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); + runtime.renew_goal(GoalKeepalivePacket { goal_id: 7 }, client); + assert!(runtime.last_command_time.is_none()); + } + + #[test] + fn freshness_uses_only_simulation_timestamps() { + assert!(fresh(Some(10_000), 60_000, IMU_FRESHNESS_MICROSECONDS)); + assert!(!fresh(Some(10_000), 60_001, IMU_FRESHNESS_MICROSECONDS)); + assert!(!fresh(Some(60_001), 60_000, IMU_FRESHNESS_MICROSECONDS)); + assert!(!fresh(None, 60_000, IMU_FRESHNESS_MICROSECONDS)); + } + + #[test] + fn clock_regression_resets_the_simulator_epoch() { + let mut runtime = SitlRuntime::new().unwrap(); + runtime.simulation_time = Some(20_000); + runtime.last_imu_time = Some(20_000); + runtime.last_local_state_time = Some(20_000); + runtime.last_command_time = Some(20_000); + runtime.pending_control_step = Some((20_000, Some(0.01))); + runtime.active_goal = Some(GoalPacket { + goal_id: 7, + north_metres: 0.0, + east_metres: 0.0, + up_metres: 1.0, + }); + assert_eq!( + runtime.advance_simulation_clock(10_000), + ClockWork::Supervise + ); + + assert_eq!(runtime.simulation_time, Some(10_000)); + assert!(runtime.estimator.is_none()); + assert!(runtime.active_goal.is_none()); + assert!(runtime.last_imu_time.is_none()); + assert!(runtime.last_local_state_time.is_none()); + assert!(runtime.last_command_time.is_none()); + assert!(runtime.pending_control_step.is_none()); + assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); + } + + #[test] + fn only_the_matching_clock_tick_releases_an_imu_control_step() { + let mut runtime = SitlRuntime::new().unwrap(); + runtime.pending_control_step = Some((20_000, Some(0.01))); + + assert_eq!( + runtime.advance_simulation_clock(17_500), + ClockWork::Supervise + ); + assert_eq!(runtime.pending_control_step, Some((20_000, Some(0.01)))); + assert_eq!( + runtime.advance_simulation_clock(20_000), + ClockWork::Control(Some(0.01)) + ); + assert!(runtime.pending_control_step.is_none()); + } + + #[test] + fn simulator_epoch_reset_accepts_a_new_safety_clock_epoch() { + let mut runtime = SitlRuntime::new().unwrap(); + let healthy = SafetyInputs { + estimator_healthy: true, + imu_fresh: true, + link_fresh: true, + actuator_healthy: true, + }; + runtime + .safety + .request_arm(Timestamp::from_microseconds(1_000), healthy); + assert_eq!( + runtime + .safety + .evaluate( + Timestamp::from_microseconds(1_000 + SITL_PREARM_DWELL_MICROSECONDS), + healthy, + ) + .state(), + kaptan_safety::SafetyState::Armed + ); + + runtime.reset_for_simulator_epoch(); + + assert_eq!(runtime.safety.state(), kaptan_safety::SafetyState::Disarmed); + assert_eq!( + runtime + .safety + .request_arm(Timestamp::from_microseconds(1), healthy) + .state(), + kaptan_safety::SafetyState::PreArm + ); + } +} diff --git a/tools/sitl/src/protocol.rs b/tools/sitl/src/protocol.rs new file mode 100644 index 0000000..d829ef4 --- /dev/null +++ b/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 { + 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 { + 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 { + 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 { + 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 { + decode_goal_with_magic(bytes, LAND_MAGIC, true) +} + +fn decode_goal_with_magic( + bytes: &[u8], + magic: [u8; 4], + require_ground_plane: bool, +) -> Result { + 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 { + 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 { + Some(u32::from_le_bytes(bytes.try_into().ok()?)) +} + +fn read_u64(bytes: &[u8]) -> Option { + Some(u64::from_le_bytes(bytes.try_into().ok()?)) +} + +fn read_f32(bytes: &[u8]) -> Option { + 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)); + } +}