ublox_core/
lib.rs

1/*
2Copyright (c) 2020 Todd Stellanova
3LICENSE: BSD3 (see LICENSE file)
4*/
5
6#![no_std]
7
8use embedded_hal as hal;
9
10mod interface;
11pub use interface::{DeviceInterface, SerialInterface};
12
13use hal::blocking::delay::DelayUs;
14
15#[allow(unused)]
16mod messages;
17use messages::*;
18
19/// Errors in this crate
20#[derive(Debug)]
21pub enum Error<CommE> {
22    /// Sensor communication error
23    Comm(CommE),
24
25    /// Sensor is not responding
26    Unresponsive,
27}
28
29pub fn new_serial_driver<UART, CommE>(
30    uart: UART,
31) -> UbxDriver<SerialInterface<UART>>
32where
33    UART: hal::serial::Read<u8, Error = CommE>,
34    CommE: core::fmt::Debug,
35{
36    let iface = interface::SerialInterface::new(uart);
37    UbxDriver::new_with_interface(iface)
38}
39
40/// Read buffer size based on maximum UBX message size we support
41const READ_BUF_LEN: usize = 128;
42
43pub struct UbxDriver<DI> {
44    /// the device interface
45    di: DI,
46    read_buf: [u8; READ_BUF_LEN],
47
48    /// The last received UBX-NAV-PVT from the device, if any
49    last_nav_pvt: Option<NavPosVelTimeM8>,
50    /// The last received UBX-MON-HW from the device, if any
51    last_mon_hw: Option<MonHardwareM8>,
52    /// The last received UBX-NAV-DOP from the device, if any
53    last_nav_dop: Option<NavDopM8>,
54}
55
56impl<DI, CommE> UbxDriver<DI>
57where
58    DI: DeviceInterface<InterfaceError = Error<CommE>>,
59    CommE: core::fmt::Debug,
60{
61    pub(crate) fn new_with_interface(device_interface: DI) -> Self {
62        Self {
63            di: device_interface,
64            read_buf: [0; READ_BUF_LEN],
65            last_nav_pvt: None,
66            last_mon_hw: None,
67            last_nav_dop: None,
68        }
69    }
70
71    pub fn setup(
72        &mut self,
73        _delay_source: &mut impl DelayUs<u32>,
74    ) -> Result<(), DI::InterfaceError> {
75        //TODO configure ublox sensor using CFG message
76        Ok(())
77    }
78
79    pub fn take_last_nav_pvt(&mut self) -> Option<NavPosVelTimeM8> {
80        self.last_nav_pvt.take()
81    }
82
83    pub fn take_last_nav_dop(&mut self) -> Option<NavDopM8> {
84        self.last_nav_dop.take()
85    }
86
87    pub fn take_last_mon_hw(&mut self) -> Option<MonHardwareM8> {
88        self.last_mon_hw.take()
89    }
90
91    /// generate a 16 bit checksum for a payload
92    fn checksum_for_payload(
93        payload: &[u8],
94        _dump_ck: bool,
95    ) -> [u8; UBX_CKSUM_LEN] {
96        let mut checksum = [0u8; UBX_CKSUM_LEN];
97        for word in payload {
98            checksum[0] = checksum[0].wrapping_add(*word);
99            checksum[1] = checksum[1].wrapping_add(checksum[0]);
100        }
101        checksum
102    }
103
104    /// Read our interface for a message of known size
105    ///
106    fn read_ubx_message(
107        &mut self,
108        msg_len: usize,
109        dump_ck: bool,
110    ) -> Result<(bool, usize), DI::InterfaceError> {
111        // The length sent in the header is defined as being that of the payload only.
112        // It does not include the Preamble, Message Class, Message ID, Length, or CRC fields.
113        // The number format of the length field is a Little-Endian unsigned 16-bit integer.
114
115        let max_pay_idx = UBX_HEADER_LEN + msg_len;
116        let max_msg_idx = max_pay_idx + UBX_CKSUM_LEN;
117        let desired_count = max_msg_idx - UBX_HEADER_LEN;
118        self.read_buf[max_msg_idx] = 0;
119        let read_count = self
120            .di
121            .read_many(&mut self.read_buf[UBX_HEADER_LEN..max_msg_idx])?;
122        if read_count < desired_count {
123            // unable to read enough bytes to fill the message struct
124            return Ok((false, 0));
125        }
126        let calc_ck =
127            Self::checksum_for_payload(&self.read_buf[..max_pay_idx], dump_ck);
128        let recvd_ck =
129            &self.read_buf[(max_msg_idx - UBX_CKSUM_LEN)..max_msg_idx];
130        let matches = calc_ck[0] == recvd_ck[0] && calc_ck[1] == recvd_ck[1];
131        if matches {
132            Ok((true, max_pay_idx))
133        } else {
134            Ok((false, 0))
135        }
136    }
137
138    /// Read a UBX-NAV-PVT message from the device
139    fn handle_msg_nav_pvt(&mut self) -> Result<(), DI::InterfaceError> {
140        let (ck_ok, max_pay_idx) =
141            self.read_ubx_message(UBX_MSG_LEN_NAV_PVT, false)?;
142        if ck_ok {
143            self.last_nav_pvt = messages::nav_pvt_from_bytes(
144                &self.read_buf[UBX_HEADER_LEN..max_pay_idx],
145            );
146        }
147        Ok(())
148    }
149
150    /// Read a UBX-NAV-DOP message from the device
151    fn handle_msg_nav_dop(&mut self) -> Result<(), DI::InterfaceError> {
152        let (ck_ok, max_pay_idx) =
153            self.read_ubx_message(UBX_MSG_LEN_NAV_DOP, true)?;
154        if ck_ok {
155            self.last_nav_dop = messages::nav_dop_from_bytes(
156                &self.read_buf[UBX_HEADER_LEN..max_pay_idx],
157            );
158        }
159        Ok(())
160    }
161
162    /// Read a UBX-MON-HW message from the device
163    fn handle_msg_mon_hw(&mut self) -> Result<(), DI::InterfaceError> {
164        let (ck_ok, max_pay_idx) =
165            self.read_ubx_message(UBX_MSG_LEN_MON_HW, false)?;
166        if ck_ok {
167            self.last_mon_hw = messages::mon_hw_from_bytes(
168                &self.read_buf[UBX_HEADER_LEN..max_pay_idx],
169            );
170        }
171        Ok(())
172    }
173
174    /// Handle a message we don't recognize, by reading past it
175    fn skip_unhandled_msg(&mut self) -> Result<(), DI::InterfaceError> {
176        // The length sent in the header is defined as being that of the payload only.
177        // It does not include the Preamble, Message Class, Message ID, Length, or CRC fields.
178        // The number format of the length field is a Little-Endian unsigned 16-bit integer.
179        let msg_len = ((self.read_buf[2] as u16)
180            + ((self.read_buf[3] as u16) << 8)) as usize;
181        let max_pay_idx = UBX_HEADER_LEN + msg_len;
182        let max_msg_idx = (max_pay_idx + UBX_CKSUM_LEN).min(READ_BUF_LEN);
183        self.di
184            .read_many(&mut self.read_buf[UBX_HEADER_LEN..max_msg_idx])?;
185
186        Ok(())
187    }
188
189    pub fn handle_all_messages(
190        &mut self,
191        delay_source: &mut impl DelayUs<u32>,
192    ) -> Result<usize, DI::InterfaceError> {
193        let mut msg_count = 0;
194        loop {
195            let handled_count = self.handle_one_message()?;
196            if handled_count > 0 {
197                msg_count += handled_count;
198            } else {
199                break;
200            }
201            delay_source.delay_us(1000);
202        }
203        return Ok(msg_count);
204    }
205
206    /// return 1 if we handled a message?
207    pub fn handle_one_message(&mut self) -> Result<usize, DI::InterfaceError> {
208        let mut msg_idx = 0;
209        // fill our incoming message buffer to avoid overruns
210        let available = self.di.fill();
211        if available < UBX_MIN_MSG_LEN {
212            return Ok(0);
213        }
214
215        loop {
216            if msg_idx < 2 {
217                let byte = self.di.read()?;
218                if byte == UBX_PRELUDE_BYTES[msg_idx] {
219                    msg_idx += 1;
220                } else {
221                    // reset: the byte doesn't match the prelude sequence
222                    msg_idx = 0;
223                }
224            } else {
225                let rc =
226                    self.di.read_many(&mut self.read_buf[..UBX_HEADER_LEN]);
227                let header_fail = match rc {
228                    Ok(read_count) => read_count != UBX_HEADER_LEN,
229                    _ => true,
230                };
231                if header_fail {
232                    return Ok(0);
233                }
234
235                let msg_unique_id: u16 =
236                    (self.read_buf[0] as u16) << 8 | (self.read_buf[1] as u16);
237                return match msg_unique_id {
238                    UBX_MSG_ID_NAV_PVT => {
239                        self.handle_msg_nav_pvt()?;
240                        Ok(1)
241                    }
242                    UBX_MSG_ID_NAV_DOP => {
243                        self.handle_msg_nav_dop()?;
244                        Ok(1)
245                    }
246                    UBX_MSG_ID_MON_HW => {
247                        self.handle_msg_mon_hw()?;
248                        Ok(1)
249                    }
250                    _ => {
251                        // unhandled message type...skip to next message
252                        self.skip_unhandled_msg()?;
253                        Ok(1)
254                    }
255                };
256            }
257        }
258    }
259}
ublox_core/lib.rs

ublox_core/
lib.rs
