1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124

//! Decode aircraft positions encoded in Compact Position Reporting (CPR) format.

use crate::types::{CPRFrame, Parity, Position};
use std::cmp;
use std::f64::consts::PI;

const NZ: f64 = 15.0;
const D_LAT_EVEN: f64 = 360.0 / (4.0 * NZ);
const D_LAT_ODD: f64 = 360.0 / (4.0 * NZ - 1.0);
const CPR_MAX: f64 = 131_072.0;

fn cpr_nl(lat: f64) -> u64 {
    let x = 1.0 - (PI / (2.0 * NZ)).cos();
    let y = ((PI / 180.0) * lat).cos().powi(2);
    ((2.0 * PI) / (1.0 - (x / y)).acos()).floor() as u64
}

/// Calculates a globally unambiguous position based on a pair of frames containing position information
/// encoded in CPR format. A position is returned when passed a tuple containing two frames of opposite parity
/// (even and odd). The frames in the tuple should be ordered according to when they were received: the first
/// frame being the oldest frame and the second frame being the latest.
pub fn get_position(cpr_frames: (&CPRFrame, &CPRFrame)) -> Option<Position> {
    let latest_frame = cpr_frames.1;
    let (even_frame, odd_frame) = match cpr_frames {
        (
            even
            @ CPRFrame {
                parity: Parity::Even,
                ..
            },
            odd @ CPRFrame {
                parity: Parity::Odd,
                ..
            },
        )
        | (
            odd @ CPRFrame {
                parity: Parity::Odd,
                ..
            },
            even
            @ CPRFrame {
                parity: Parity::Even,
                ..
            },
        ) => (even, odd),
        _ => return None,
    };

    let cpr_lat_even = even_frame.position.latitude / CPR_MAX;
    let cpr_lon_even = even_frame.position.longitude / CPR_MAX;
    let cpr_lat_odd = odd_frame.position.latitude / CPR_MAX;
    let cpr_lon_odd = odd_frame.position.longitude / CPR_MAX;

    let j = (59.0 * cpr_lat_even - 60.0 * cpr_lat_odd + 0.5).floor();

    let mut lat_even = D_LAT_EVEN * (j % 60.0 + cpr_lat_even);
    let mut lat_odd = D_LAT_ODD * (j % 59.0 + cpr_lat_odd);

    if lat_even >= 270.0 {
        lat_even -= 360.0;
    }

    if lat_odd >= 270.0 {
        lat_odd -= 360.0;
    }

    let lat = if latest_frame == even_frame {
        lat_even
    } else {
        lat_odd
    };

    let (lat, lon) = get_lat_lon(lat, cpr_lon_even, cpr_lon_odd, &latest_frame.parity);

    Some(Position {
        latitude: lat,
        longitude: lon,
    })
}

fn get_lat_lon(lat: f64, cpr_lon_even: f64, cpr_lon_odd: f64, parity: &Parity) -> (f64, f64) {
    let (p, c) = if parity == &Parity::Even {
        (0, cpr_lon_even)
    } else {
        (1, cpr_lon_odd)
    };
    let ni = cmp::max(cpr_nl(lat) - p, 1) as f64;
    let m =
        (cpr_lon_even * (cpr_nl(lat) - 1) as f64 - cpr_lon_odd * cpr_nl(lat) as f64 + 0.5).floor();
    let mut lon = (360.0 / ni) * (m % ni + c);
    if lon >= 180.0 {
        lon -= 360.0;
    }
    (lat, lon)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn cpr_calculate_position() {
        let odd = CPRFrame {
            position: Position {
                latitude: 74158.0,
                longitude: 50194.0,
            },
            parity: Parity::Odd,
        };

        let even = CPRFrame {
            position: Position {
                latitude: 93000.0,
                longitude: 51372.0,
            },
            parity: Parity::Even,
        };

        let position = get_position((&odd, &even)).unwrap();
        assert_eq!(position.latitude, 52.25720214843750);
        assert_eq!(position.longitude, 3.91937255859375);
    }
}