libs/kaptan-drivers: add F722 sensor services

libs/kaptan-drivers/src/mtf01/mod.rs

@@ -0,0 +1,471 @@
+//! MicoAir MTF-01 Microlink UART frame decoder.
+//!
+//! Device documentation belongs beside this module.
+//!
+//! The MTF-01 continuously transmits fixed-size Microlink range-sensor frames.
+//! Feed each received UART byte to [`Mtf01::receive_byte`]. The parser is
+//! allocation-free, does not use packed structs, and resynchronizes on a new
+//! frame header after malformed input.
+
+use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
+use kaptan_time::{MonotonicClock, Timestamp};
+
+const FRAME_HEADER: u8 = 0xEF;
+const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51;
+const PAYLOAD_LENGTH: usize = 20;
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+enum State {
+    Header,
+    DeviceId,
+    SystemId,
+    MessageId,
+    Sequence,
+    Length,
+    Payload { index: usize },
+    Checksum,
+}
+
+/// One complete MTF-01 range and optical-flow report.
+///
+/// This preserves the MTF-01's device-specific fields. Use
+/// [`Self::range_snapshot`] or [`Self::optical_flow_snapshot`] to obtain a
+/// device-independent observation for the rest of the flight stack.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct Mtf01Report {
+    /// Flight-controller monotonic time at frame reception.
+    pub timestamp: Timestamp,
+    /// MTF-01 system time, in milliseconds.
+    pub sensor_time_milliseconds: u32,
+    /// Measured distance in millimetres. Zero means unavailable.
+    pub distance_millimetres: u32,
+    /// MTF-01 range-signal strength, from 0 through 255.
+    pub distance_strength: u8,
+    /// MTF-01 distance precision; smaller values indicate higher accuracy.
+    pub distance_precision: u8,
+    /// Whether the MTF-01 marked the distance measurement valid.
+    pub distance_valid: bool,
+    /// Optical-flow velocity along the MTF-01 X axis, in centimetres/second
+    /// at one metre above ground.
+    pub flow_velocity_x_centimetres_per_second_at_one_metre: i16,
+    /// Optical-flow velocity along the MTF-01 Y axis, in centimetres/second
+    /// at one metre above ground.
+    pub flow_velocity_y_centimetres_per_second_at_one_metre: i16,
+    /// MTF-01 optical-flow quality, from 0 through 255.
+    pub flow_quality: u8,
+    /// Whether the MTF-01 marked the optical-flow measurement valid.
+    pub flow_valid: bool,
+}
+
+impl Mtf01Report {
+    /// Creates an MTF-01 observation from one validated Microlink frame.
+    #[allow(clippy::too_many_arguments)]
+    pub const fn new(
+        timestamp: Timestamp,
+        sensor_time_milliseconds: u32,
+        distance_millimetres: u32,
+        distance_strength: u8,
+        distance_precision: u8,
+        distance_valid: bool,
+        flow_velocity_x_centimetres_per_second_at_one_metre: i16,
+        flow_velocity_y_centimetres_per_second_at_one_metre: i16,
+        flow_quality: u8,
+        flow_valid: bool,
+    ) -> Self {
+        Self {
+            timestamp,
+            sensor_time_milliseconds,
+            distance_millimetres,
+            distance_strength,
+            distance_precision,
+            distance_valid,
+            flow_velocity_x_centimetres_per_second_at_one_metre,
+            flow_velocity_y_centimetres_per_second_at_one_metre,
+            flow_quality,
+            flow_valid,
+        }
+    }
+
+    /// Returns the valid range portion of this report.
+    pub const fn range_snapshot(self) -> Option<RangeSnapshot> {
+        if self.distance_valid && self.distance_millimetres != 0 {
+            Some(RangeSnapshot::new(
+                self.timestamp,
+                self.distance_millimetres,
+            ))
+        } else {
+            None
+        }
+    }
+
+    /// Returns the valid, ground-referenced optical-flow portion of this
+    /// report. MTF-01 velocities are specified in centimetres per second at
+    /// one metre; the device documentation scales them by measured height.
+    pub fn optical_flow_snapshot(self) -> Option<OpticalFlowSnapshot> {
+        if !self.distance_valid || self.distance_millimetres == 0 || !self.flow_valid {
+            return None;
+        }
+
+        let velocity_x = i64::from(self.flow_velocity_x_centimetres_per_second_at_one_metre)
+            * i64::from(self.distance_millimetres)
+            / 100;
+        let velocity_y = i64::from(self.flow_velocity_y_centimetres_per_second_at_one_metre)
+            * i64::from(self.distance_millimetres)
+            / 100;
+        if velocity_x < i64::from(i32::MIN)
+            || velocity_x > i64::from(i32::MAX)
+            || velocity_y < i64::from(i32::MIN)
+            || velocity_y > i64::from(i32::MAX)
+        {
+            return None;
+        }
+
+        Some(OpticalFlowSnapshot::new(
+            self.timestamp,
+            [velocity_x as i32, velocity_y as i32],
+            self.distance_millimetres,
+            self.flow_quality,
+        ))
+    }
+}
+
+/// MTF-01 Microlink decoder.
+///
+/// One decoder is owned by one UART receive path. It accepts the MTF-01's
+/// documented range-sensor message ID and 20-byte payload length. Device and
+/// system IDs are deliberately accepted as transmitted, matching MicoAir's
+/// reference decoder and allowing sensor IDs configured outside factory
+/// defaults.
+pub struct Mtf01 {
+    state: State,
+    checksum: u8,
+    payload: [u8; PAYLOAD_LENGTH],
+}
+
+impl Mtf01 {
+    /// Creates a decoder awaiting the `0xEF` Microlink frame header.
+    pub const fn new() -> Self {
+        Self {
+            state: State::Header,
+            checksum: 0,
+            payload: [0; PAYLOAD_LENGTH],
+        }
+    }
+
+    /// Accepts one UART byte and returns a complete, checksum-validated MTF-01
+    /// report when available.
+    ///
+    /// `timestamp` should be the controller monotonic time at receipt of this
+    /// byte. It is used only for a completed frame, so it records the arrival
+    /// time of the report rather than an arbitrary intermediate byte.
+    pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option<Mtf01Report> {
+        match self.state {
+            State::Header => {
+                if byte == FRAME_HEADER {
+                    self.checksum = byte;
+                    self.state = State::DeviceId;
+                }
+            }
+            State::DeviceId => {
+                self.accept(byte, State::SystemId);
+            }
+            State::SystemId => {
+                self.accept(byte, State::MessageId);
+            }
+            State::MessageId => {
+                if byte == RANGE_SENSOR_MESSAGE_ID {
+                    self.accept(byte, State::Sequence);
+                } else {
+                    self.restart(byte);
+                }
+            }
+            State::Sequence => self.accept(byte, State::Length),
+            State::Length => {
+                if usize::from(byte) == PAYLOAD_LENGTH {
+                    self.accept(byte, State::Payload { index: 0 });
+                } else {
+                    self.restart(byte);
+                }
+            }
+            State::Payload { index } => {
+                self.payload[index] = byte;
+                self.checksum = self.checksum.wrapping_add(byte);
+                self.state = if index + 1 == PAYLOAD_LENGTH {
+                    State::Checksum
+                } else {
+                    State::Payload { index: index + 1 }
+                };
+            }
+            State::Checksum => {
+                let valid = byte == self.checksum;
+                // The checksum terminates the frame even when its value is
+                // also the Microlink header. The reference parser returns to
+                // its initial state here rather than treating checksum data as
+                // the start of the next frame.
+                self.reset();
+                if valid {
+                    return Some(self.snapshot(timestamp));
+                }
+            }
+        }
+
+        None
+    }
+
+    /// Drops any partial frame and awaits a new header.
+    pub fn reset(&mut self) {
+        self.state = State::Header;
+        self.checksum = 0;
+    }
+
+    fn accept(&mut self, byte: u8, next: State) {
+        self.checksum = self.checksum.wrapping_add(byte);
+        self.state = next;
+    }
+
+    fn restart(&mut self, byte: u8) {
+        if byte == FRAME_HEADER {
+            self.checksum = FRAME_HEADER;
+            self.state = State::DeviceId;
+        } else {
+            self.reset();
+        }
+    }
+
+    fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report {
+        Mtf01Report::new(
+            timestamp,
+            u32::from_le_bytes(self.payload[0..4].try_into().unwrap()),
+            u32::from_le_bytes(self.payload[4..8].try_into().unwrap()),
+            self.payload[8],
+            self.payload[9],
+            self.payload[10] == 1,
+            i16::from_le_bytes(self.payload[12..14].try_into().unwrap()),
+            i16::from_le_bytes(self.payload[14..16].try_into().unwrap()),
+            self.payload[16],
+            self.payload[17] == 1,
+        )
+    }
+}
+
+impl Default for Mtf01 {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+/// Timestamping MTF-01 UART receive service.
+///
+/// Feed this service from the selected UART receive interrupt or DMA consumer.
+/// The board retains ownership of pin muxing, buffering, and interrupt
+/// acknowledgement.
+pub struct Mtf01Service<Clock> {
+    clock: Clock,
+    decoder: Mtf01,
+}
+
+impl<Clock> Mtf01Service<Clock> {
+    /// Creates an MTF-01 receive service awaiting a Microlink frame header.
+    pub const fn new(clock: Clock) -> Self {
+        Self {
+            clock,
+            decoder: Mtf01::new(),
+        }
+    }
+
+    /// Discards a partial frame after a UART transport fault.
+    pub fn reset(&mut self) {
+        self.decoder.reset();
+    }
+}
+
+impl<Clock: MonotonicClock> Mtf01Service<Clock> {
+    /// Consumes one UART byte and returns one validated MTF-01 report.
+    pub fn on_received_byte(&mut self, byte: u8) -> Option<Mtf01Report> {
+        self.decoder.receive_byte(self.clock.now(), byte)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{Mtf01, Mtf01Report, Mtf01Service};
+    use kaptan_time::{MonotonicClock, Timestamp};
+
+    fn frame(payload: [u8; 20]) -> [u8; 27] {
+        let mut frame = [0_u8; 27];
+        frame[..6].copy_from_slice(&[0xEF, 0x0F, 0x00, 0x51, 0xA5, 20]);
+        frame[6..26].copy_from_slice(&payload);
+        frame[26] = frame[..26]
+            .iter()
+            .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte));
+        frame
+    }
+
+    fn sample_frame() -> [u8; 27] {
+        frame([
+            0x78, 0x56, 0x34, 0x12, // sensor time: 305419896 ms
+            0xD2, 0x04, 0x00, 0x00, // distance: 1234 mm
+            200, 7, 1, 0, // range metadata and reserved byte
+            0x2E, 0xFB, // flow x: -1234
+            0xD2, 0x04, // flow y: 1234
+            250, 1, 0, 0, // flow metadata and reserved u16
+        ])
+    }
+
+    #[test]
+    fn decodes_a_valid_little_endian_range_and_flow_report() {
+        let mut decoder = Mtf01::new();
+        let timestamp = Timestamp::from_microseconds(9_876);
+        let mut report = None;
+        for byte in sample_frame() {
+            report = decoder.receive_byte(timestamp, byte).or(report);
+        }
+
+        assert_eq!(
+            report,
+            Some(Mtf01Report::new(
+                timestamp,
+                0x1234_5678,
+                1_234,
+                200,
+                7,
+                true,
+                -1_234,
+                1_234,
+                250,
+                true,
+            ))
+        );
+    }
+
+    #[test]
+    fn rejects_a_bad_checksum_without_publishing_a_report() {
+        let mut decoder = Mtf01::new();
+        let mut corrupt = sample_frame();
+        corrupt[26] ^= 0xFF;
+
+        assert!(corrupt.into_iter().all(|byte| {
+            decoder
+                .receive_byte(Timestamp::from_microseconds(1), byte)
+                .is_none()
+        }));
+    }
+
+    #[test]
+    fn treats_a_header_valued_checksum_as_a_checksum() {
+        let mut decoder = Mtf01::new();
+        let mut payload = [0_u8; 20];
+        // The fixed header through length sums to 0x08, making 0xE7 produce
+        // the `0xEF` checksum value that otherwise resembles a new header.
+        payload[0] = 0xE7;
+        let checksum_is_header = frame(payload);
+        assert_eq!(checksum_is_header[26], 0xEF);
+
+        let first_report = checksum_is_header
+            .into_iter()
+            .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(1), byte));
+        assert!(first_report.is_some());
+        let report = sample_frame()
+            .into_iter()
+            .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(2), byte));
+
+        assert_eq!(report.unwrap().distance_millimetres, 1_234);
+    }
+
+    #[test]
+    fn resynchronizes_when_a_header_arrives_during_a_bad_frame() {
+        let mut decoder = Mtf01::new();
+        let valid = sample_frame();
+        let mut bytes = [0_u8; 30];
+        bytes[..3].copy_from_slice(&[0xEF, 0x0F, 0xEF]);
+        bytes[3..].copy_from_slice(&valid);
+
+        let mut report = None;
+        for byte in bytes {
+            report = decoder
+                .receive_byte(Timestamp::from_microseconds(7), byte)
+                .or(report);
+        }
+        assert_eq!(report.unwrap().distance_millimetres, 1_234);
+    }
+
+    #[test]
+    fn does_not_accept_another_micolink_message_type() {
+        let mut decoder = Mtf01::new();
+        let mut other_message = sample_frame();
+        other_message[3] = 0x52;
+        other_message[26] = other_message[..26]
+            .iter()
+            .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte));
+
+        assert!(other_message.into_iter().all(|byte| {
+            decoder
+                .receive_byte(Timestamp::from_microseconds(1), byte)
+                .is_none()
+        }));
+    }
+
+    #[test]
+    fn accepts_configured_micolink_device_and_system_ids() {
+        let mut decoder = Mtf01::new();
+        let mut configured_ids = sample_frame();
+        configured_ids[1] = 0x42;
+        configured_ids[2] = 0x99;
+        configured_ids[26] = configured_ids[..26]
+            .iter()
+            .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte));
+
+        let report = configured_ids
+            .into_iter()
+            .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(1), byte));
+        assert_eq!(report.unwrap().distance_millimetres, 1_234);
+    }
+
+    #[test]
+    fn exposes_only_valid_generic_observations() {
+        let mut report = Mtf01Report::new(
+            Timestamp::from_microseconds(123),
+            7,
+            2_000,
+            8,
+            9,
+            true,
+            50,
+            -25,
+            12,
+            true,
+        );
+        assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000);
+        assert_eq!(
+            report
+                .optical_flow_snapshot()
+                .unwrap()
+                .velocity_millimetres_per_second,
+            [1_000, -500]
+        );
+
+        report.distance_valid = false;
+        assert_eq!(report.range_snapshot(), None);
+        assert_eq!(report.optical_flow_snapshot(), None);
+    }
+
+    struct Clock(u64);
+
+    impl MonotonicClock for Clock {
+        fn now(&mut self) -> Timestamp {
+            self.0 += 1;
+            Timestamp::from_microseconds(self.0)
+        }
+    }
+
+    #[test]
+    fn timestamps_a_report_with_its_last_received_byte() {
+        let mut service = Mtf01Service::new(Clock(100));
+        let report = sample_frame()
+            .into_iter()
+            .find_map(|byte| service.on_received_byte(byte))
+            .unwrap();
+
+        assert_eq!(report.timestamp, Timestamp::from_microseconds(127));
+    }
+}