binex/message/record/solutions/frame/
geo.rs

1use crate::{
2    prelude::{Error, Message},
3    utils::Utils,
4};
5
6use std::str::from_utf8;
7
8#[derive(Debug, Clone, PartialEq)]
9pub struct PositionGeo3d {
10    /// longitude angle [ddeg]
11    pub long_ddeg: f64,
12    /// latitude angle [ddeg]
13    pub lat_ddeg: f64,
14    /// altitude above sea level [m]
15    pub alt_m: f64,
16    /// Ellpsoid shape
17    pub ellipsoid: String,
18}
19
20impl PositionGeo3d {
21    pub(crate) fn encoding_size(&self) -> usize {
22        let mut size = 24;
23        if !self.ellipsoid.eq("WGS84") {
24            let str_len = self.ellipsoid.len();
25            size += Message::bnxi_encoding_size(str_len as u32);
26            size += str_len;
27        } else {
28            size += 1;
29        }
30        size
31    }
32
33    pub(crate) fn encode(&self, big_endian: bool, buf: &mut [u8]) -> Result<usize, Error> {
34        let mut ptr = 0;
35        let size = self.encoding_size();
36
37        if buf.len() < size {
38            return Err(Error::NotEnoughBytes);
39        }
40
41        // encode ellipsod
42        if self.ellipsoid.eq("WGS84") {
43            buf[ptr] = 0;
44            ptr += 1;
45        } else {
46            let str_len = self.ellipsoid.len();
47
48            let size = Message::encode_bnxi(str_len as u32, big_endian, &mut buf[ptr..])?;
49            ptr += size;
50
51            buf[ptr..ptr + str_len].copy_from_slice(self.ellipsoid.as_bytes());
52            ptr += str_len;
53        }
54
55        // encode (x, y, z)
56        let bytes = if big_endian {
57            self.long_ddeg.to_be_bytes()
58        } else {
59            self.long_ddeg.to_le_bytes()
60        };
61
62        buf[ptr..ptr + 8].copy_from_slice(&bytes);
63
64        let bytes = if big_endian {
65            self.lat_ddeg.to_be_bytes()
66        } else {
67            self.lat_ddeg.to_le_bytes()
68        };
69
70        buf[ptr + 8..ptr + 16].copy_from_slice(&bytes);
71
72        let bytes = if big_endian {
73            self.alt_m.to_be_bytes()
74        } else {
75            self.alt_m.to_le_bytes()
76        };
77
78        buf[ptr + 16..ptr + 24].copy_from_slice(&bytes);
79        Ok(size)
80    }
81
82    pub(crate) fn decode(big_endian: bool, buf: &[u8]) -> Result<Self, Error> {
83        let buf_len = buf.len();
84        if buf_len < 2 {
85            return Err(Error::NotEnoughBytes);
86        }
87
88        let (str_len, mut ptr) = Message::decode_bnxi(buf, big_endian);
89        let str_len = str_len as usize;
90
91        let ellipsoid = if str_len == 0 {
92            "WGS84".to_string()
93        } else {
94            let ellipsoid_str =
95                from_utf8(&buf[ptr..ptr + str_len]).map_err(|_| Error::Utf8Error)?;
96
97            ellipsoid_str.to_string()
98        };
99
100        ptr += str_len;
101
102        if buf_len - str_len < 24 {
103            return Err(Error::NotEnoughBytes);
104        }
105
106        let long_ddeg = Utils::decode_f64(big_endian, &buf[ptr..ptr + 8])?;
107        let lat_ddeg = Utils::decode_f64(big_endian, &buf[ptr + 8..ptr + 16])?;
108        let alt_m = Utils::decode_f64(big_endian, &buf[ptr + 16..ptr + 24])?;
109
110        Ok(Self {
111            long_ddeg,
112            lat_ddeg,
113            alt_m,
114            ellipsoid,
115        })
116    }
117
118    /// Defines new [PositionGeo3d] expressed in WGS84 ellipsoid.
119    pub fn new_wgs84(long_ddeg: f64, lat_ddeg: f64, alt_m: f64) -> Self {
120        Self {
121            long_ddeg,
122            lat_ddeg,
123            alt_m,
124            ellipsoid: "WGS84".to_string(),
125        }
126    }
127}
128
129#[derive(Debug, Clone, PartialEq)]
130pub struct VelocityNED3d {
131    /// Northern component
132    pub north_m_s: f64,
133    /// Eastern component
134    pub east_m_s: f64,
135    /// Upwards component
136    pub up_m_s: f64,
137}
binex/message/record/solutions/frame/geo.rs

binex/message/record/solutions/frame/
geo.rs
