diff --git a/libadsb_deku/src/adsb.rs b/libadsb_deku/src/adsb.rs index 048662b..3881290 100644 --- a/libadsb_deku/src/adsb.rs +++ b/libadsb_deku/src/adsb.rs @@ -1021,6 +1021,8 @@ impl fmt::Display for VerticalRateSource { #[derive(Debug, PartialEq, Eq, DekuRead, Copy, Clone)] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct SurfacePosition { + #[deku(bits = "5")] + pub tc: u8, #[deku(bits = "7")] pub mov: u8, pub s: StatusForGroundTrack, diff --git a/libadsb_deku/src/cpr.rs b/libadsb_deku/src/cpr.rs index 45ede07..e8d441a 100644 --- a/libadsb_deku/src/cpr.rs +++ b/libadsb_deku/src/cpr.rs @@ -18,7 +18,7 @@ use core::{ #[cfg(not(feature = "alloc"))] use std::cmp; -use crate::{Altitude, CPRFormat}; +use crate::{adsb::SurfacePosition, Altitude, CPRFormat}; const NZ: f64 = 15.0; const D_LAT_EVEN: f64 = 360.0 / (4.0 * NZ); @@ -298,6 +298,34 @@ fn get_lat_lon( (lat, lon) } +/// Calculate surface position with a single surface position message (even/odd any) +/// using a reference latitude and longitude. +/// The reference point should be within 45 NM of the position. +pub fn surface_position_with_reference( + surface_position: &SurfacePosition, + reference_lat_long: (f64, f64), +) -> Option { + let i = if surface_position.f == CPRFormat::Even { 0.0 } else { 1.0 }; + + let cpr_lat = surface_position.lat_cpr as f64 / CPR_MAX; + let cpr_lon = surface_position.lon_cpr as f64 / CPR_MAX; + + let dlat = 90.0 / (60.0 - i); + let j = (reference_lat_long.0 / dlat).floor() + + (((reference_lat_long.0 % dlat) / dlat) - cpr_lat + 0.5).floor(); + + let lat = dlat * (j + cpr_lat); + + let nl = cpr_nl(lat); + let dlon = 90.0 / std::cmp::max(nl - i as u64, 1) as f64; + + let m = (reference_lat_long.1 / dlon).floor() + + (((reference_lat_long.1 % dlon) / dlon) - cpr_lon + 0.5).floor(); + let lon = dlon * (m + cpr_lon); + + Some(Position { latitude: lat, longitude: lon }) +} + #[cfg(test)] mod tests { use super::*; diff --git a/libadsb_deku/tests/file.rs b/libadsb_deku/tests/file.rs index de4206e..c3f8912 100644 --- a/libadsb_deku/tests/file.rs +++ b/libadsb_deku/tests/file.rs @@ -1,4 +1,7 @@ -use adsb_deku::Frame; +use adsb_deku::{ + cpr::{self, Position}, + Frame, +}; use test_log::test; const TEST_STR: &str = include_str!("../tests/lax-messages.txt"); @@ -17,3 +20,30 @@ fn lax_messages() { assert_ne!("{}", format!("{frame}")); } } + +#[test] +fn test_surface_position() { + let bytes = hex::decode("8c8013af3dc9e656539852a9618e").unwrap(); + let frame = Frame::from_bytes(&bytes).unwrap(); + match frame.df { + adsb_deku::DF::ADSB(a) => match a.me { + adsb_deku::adsb::ME::SurfacePosition(surface_position) => { + assert_eq!( + cpr::surface_position_with_reference( + &surface_position, + (26.828576952763022, 75.80601239775218) + ), + Some(Position { latitude: 26.823504173149495, longitude: 75.80336644099309 }) + ); + assert_eq!(surface_position.lat_cpr, 76585); + assert_eq!(surface_position.lon_cpr, 104530); + } + x => { + panic!("Unexpected message: {:?}", x) + } + }, + x => { + panic!("Unexpected message: {:?}", x) + } + } +} diff --git a/rsadsb_common/src/lib.rs b/rsadsb_common/src/lib.rs index acabd3c..51640f5 100644 --- a/rsadsb_common/src/lib.rs +++ b/rsadsb_common/src/lib.rs @@ -15,7 +15,9 @@ use core::{ #[cfg(feature = "std")] use std::time::SystemTime; -use adsb_deku::adsb::{AirborneVelocity, Identification, ME}; +use adsb_deku::adsb::{ + AirborneVelocity, Identification, StatusForGroundTrack, SurfacePosition, ME, +}; use adsb_deku::{cpr, Altitude, CPRFormat, Frame, DF, ICAO}; use tracing::{debug, info, warn}; @@ -133,6 +135,9 @@ impl Airplanes { | ME::AirbornePositionBaroAltitude(altitude) => { self.update_position(adsb.icao, altitude, lat_long, max_rang) } + ME::SurfacePosition(surface_position) => { + self.update_surface_position(adsb.icao, surface_position, lat_long) + } _ => Added::No, }; let incr_airplane_added = self.incr_messages(adsb.icao); @@ -330,6 +335,44 @@ impl Airplanes { airplane_added } + + /// update from `ME::SurfacePosition` + /// + /// Return true if entry was added into `Airplanes` + /// + fn update_surface_position( + &mut self, + icao: ICAO, + surface_position: &SurfacePosition, + lat_long: (f64, f64), + ) -> Added { + let (state, airplane_added) = self.entry_or_insert(icao); + state.coords.position = cpr::surface_position_with_reference(surface_position, lat_long); + let speed = get_surface_speed(surface_position); + state.speed = speed; + state.heading = if surface_position.s == StatusForGroundTrack::Valid { + Some(surface_position.trk as f32 * 360.0 / 128.0) + } else { + None + }; + airplane_added + } +} + +fn get_surface_speed(surface_position: &SurfacePosition) -> Option { + let speed = match surface_position.mov { + 0 => None, + 1 => Some(0.0), + 2..=8 => Some(0.125 + (surface_position.mov + 1 - 2) as f32 * 0.125), + 9..=12 => Some(1.0 + (surface_position.mov as f32 + 1.0 - 9.0) * 0.25), + 13..=38 => Some(2.0 + (surface_position.mov as f32 + 1.0 - 13.0) * 0.5), + 39..=93 => Some(15.0 + (surface_position.mov as f32 + 1.0 - 39.0) * 1.0), + 94..=108 => Some(70.0 + (surface_position.mov as f32 + 1.0 - 94.0) * 2.0), + 109..=123 => Some(100.0 + (surface_position.mov as f32 + 1.0 - 109.0) * 5.0), + 124 => Some(175.0), + _ => Some(200.0), + }; + speed } /// Generated by `Airplanes::aircraft_details()`