Add surface position support

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,

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<Position> {
+    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::*;

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)
+        }
+    }
+}

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<f32> {
+    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()`