472 lines
15 KiB
Rust
472 lines
15 KiB
Rust
//! 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::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));
|
|
}
|
|
}
|