stm_32/peripherals/ublox.rs
1// ******************************************************************************
2// * File : platforms/stm_32/src/peripherals/ublox.rs
3// * Date : June 28, 2026
4// ******************************************************************************
5// *
6// * Copyright (c) 2023, AeroVironment, Inc.
7// * All rights reserved.
8// *
9// * Redistribution and use in source and binary forms, with or without
10// * modification, are permitted provided that the following conditions are met:
11// *
12// * 1.Redistributions of source code must retain the above copyright notice, this
13// * list of conditions and the following disclaimer.
14// *
15// * 2.Redistributions in binary form must reproduce the above copyright notice,
16// * this list of conditions and the following disclaimer in the documentation
17// * and/or other materials provided with the distribution.
18// *
19// * 3.Neither the name of the copyright holder nor the names of its
20// * contributors may be used to endorse or promote products derived from
21// * this software without specific prior written permission.
22// *
23// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24// * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25// * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26// * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
27// * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
28// * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29// * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
31// * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
32// * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33// *
34// ******************************************************************************
35
36use embassy_stm32::mode::Async;
37use embassy_stm32::usart;
38use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
39use embassy_sync::signal::Signal;
40use embassy_time::Duration;
41use embassy_time::Instant;
42use embassy_time::with_timeout;
43
44use super::pps;
45use veloxity_core::errors;
46use veloxity_core::packets;
47
48const BUFFER_LEN: usize = 512;
49
50fn unix_seconds_from_utc(year: u16, month: u8, day: u8, hour: u8, min: u8, sec: u8) -> i64 {
51 let mut y = year as i32;
52 let m = month as i32;
53 y -= (m <= 2) as i32;
54 let era = if y >= 0 { y } else { y - 399 } / 400;
55 let yoe = y - era * 400;
56 let mp = m + if m > 2 { -3 } else { 9 };
57 let doy = (153 * mp + 2) / 5 + day as i32 - 1;
58 let doe = yoe * 365 + yoe / 4 - yoe / 100 + doy;
59 let days = era * 146097 + doe - 719468;
60 days as i64 * 86_400 + hour as i64 * 3_600 + min as i64 * 60 + sec as i64
61}
62
63pub static GNSS_SIGNAL: Signal<
64 CriticalSectionRawMutex,
65 Result<packets::GNSSPacket, errors::SensorError>,
66> = Signal::<CriticalSectionRawMutex, Result<packets::GNSSPacket, errors::SensorError>>::new();
67
68#[repr(C, packed)]
69#[derive(Copy, Clone)]
70pub struct PvtPayload {
71 pub i_tow: u32, // ms, GPS time of week
72 pub year: u16,
73 pub month: u8,
74 pub day: u8,
75 pub hour: u8,
76 pub min: u8,
77 pub sec: u8, // UTC
78
79 pub valid: u8, // validity flags
80 pub t_acc: u32, // ns, time accuracy estimate
81 pub nano: i32, // ns, Fraction of second -1e9 to 1e9 (UTC)
82 pub fix_type: u8, // 0 none, 1 dead reckoning, 2 2D, 3 3D, 4 GNS+dead reckoning combined, 5 time only fix
83
84 pub flags: u8,
85 pub flags2: u8,
86 pub num_sv: u8, // satellites used in solution
87 pub lon: i32,
88 pub lat: i32, // degx10^-7
89 pub height: i32,
90 pub h_msl: i32, // mm
91 pub h_acc: u32,
92 pub v_acc: u32, // mm
93 pub vel_n: i32,
94 pub vel_e: i32,
95 pub vel_d: i32,
96 pub g_speed: i32, // mm/s velocity
97 pub head_mot: i32, // degx10^-5
98 pub s_acc: u32, // mm/s speed accuracy estimate
99 pub head_acc: u32, // degx10^-5
100 pub p_dop: u16, // 0.01 (percent)
101
102 flags3: u16,
103 pub reserved1: u8,
104 pub head_veh: i32, // degx10^-5, vehicle heading
105 pub mag_dec: i16, // degx 10^-2
106 pub mag_acc: u32, // degx 10^-2
107}
108
109#[repr(C, packed)]
110#[derive(Copy, Clone)]
111pub union PvtUnion {
112 pub packet: PvtPayload,
113 pub payload: [u8; 92],
114}
115
116#[repr(u32)]
117#[derive(Clone, Copy)]
118pub enum Bitrate {
119 Baud9600 = 9600u32,
120 Baud38400 = 38400u32,
121 Baud57600 = 57600u32,
122 Baud115200 = 115200u32,
123 Baud230400 = 230400u32,
124}
125
126pub enum Protocol {
127 M8,
128 M9,
129}
130static UBX_MAX_PAYLOAD_BYTES: usize = 256;
131
132#[repr(C, packed)]
133#[derive(Copy, Clone)]
134pub struct UbxFrame {
135 cl: u8,
136 id: u8,
137 length: usize,
138 a: u8, // checksum
139 b: u8, // checksum
140 payload: [u8; UBX_MAX_PAYLOAD_BYTES],
141}
142
143pub struct UbloxSensor {
144 pub uart: usart::Uart<'static, Async>,
145 pub protocol: Protocol,
146 pub baudrate: Bitrate,
147 pub nav_period_ms: u16,
148}
149
150static PPS_PERIOD_US: u32 = 1000000u32;
151
152// checksum is over class, id, length, payload only
153fn checksum(buffer: &[u8]) -> (u8, u8) {
154 let mut ck_a: u8 = 0;
155 let mut ck_b: u8 = 0;
156
157 for &byte in buffer {
158 ck_a = ck_a.wrapping_add(byte);
159 ck_b = ck_b.wrapping_add(ck_a);
160 }
161
162 (ck_a, ck_b)
163}
164
165fn make_packet(class: u8, id: u8, payload: &[u8], buffer: &mut [u8]) -> bool {
166 let length = payload.len();
167
168 // Check for payload too big
169 if length + 8 > buffer.len() {
170 return false;
171 }
172
173 // header
174 buffer[0] = 0xB5;
175 buffer[1] = 0x62;
176 buffer[2] = class;
177 buffer[3] = id;
178 buffer[4..6].copy_from_slice(&(length as u16).to_le_bytes());
179
180 // payload
181 buffer[6..length + 6].copy_from_slice(payload);
182
183 // checksum
184 let (ck_a, ck_b) = checksum(&buffer[2..length + 6]);
185 buffer[length + 6] = ck_a;
186 buffer[length + 7] = ck_b;
187
188 true
189}
190
191impl UbloxSensor {
192 async fn tx(&mut self, class: u8, id: u8, payload: &[u8]) -> bool {
193 let mut buffer = [0u8; BUFFER_LEN]; // largest ubx packet length we will support
194 // Make packet
195 if make_packet(class, id, payload, &mut buffer) {
196 // Make the expected ack packet
197 let mut ack: [u8; 10] = [0u8; 10];
198 let _result = make_packet(0x05, 0x01, &[class, id], &mut ack); // ack packet is class=0x05 id=0x01
199
200 // send packet
201 let result = self.uart.write(&buffer[0..payload.len() + 8]).await;
202 if let Ok(_size) = result {
203 // check it it was successful
204 let result = self.look_for_ack(&ack).await;
205 return result;
206 }
207 }
208 false
209 }
210
211 async fn cfg_prt(&mut self, _baud: u32) -> bool {
212 match self.protocol {
213 Protocol::M8 => {
214 let mut payload = [0u8; 20]; // #define CFG_PRT_LENGTH 20
215 payload[0] = 0x01; // Port 1 is the UART
216 //payload[1] = 0x00; // Reserved
217 //payload[2] = 0x00; // txReady
218 //payload[3] = 0x00; // txReady
219 payload[4] = 0xC0; // mode 1100 0000 (8-bit character length)
220 payload[5] = 0x08; // mode 0000 1000 (No parity, 1 stop bit)
221 //payload[6] = 0x00; // mode
222 //payload[7] = 0x00; // mode
223 payload[8..12].copy_from_slice(&(self.baudrate as u32).to_le_bytes()); // meas rate
224 payload[12] = 0x01; // inProtoMask (ubx)
225 //payload[13] = 0x00; // inProtoMask
226 payload[14] = 0x01; // outProtoMask (ubx)
227 //payload[15] = 0x00; // outProtoMask
228 //payload[16] = 0x00; // flags
229 //payload[17] = 0x00; // flags
230 //payload[18] = 0x00; // reserved2
231 //payload[19] = 0x00; // reserved2
232
233 self.tx(0x06, 0x00, &payload).await
234 }
235 Protocol::M9 => {
236 let mut payload = [0u8; 34];
237 //payload[0] = 0x00; // Message Version (1 bytes)
238 payload[1] = 0x01; // Write to RAM bit 1 is ram, 2 is bbr layer, 3 is flash (1 bytes)
239 //payload[2] = 0x00; // transaction/action (1 bytes)
240 //payload[3] = 0x00; // reserved0 (1 bytes)
241
242 // Key-Value pairs
243 // baud rate (8 bytes)
244 payload[4..8].copy_from_slice(&0x40520001u32.to_le_bytes());
245 payload[8..12].copy_from_slice(&(self.baudrate as u32).to_le_bytes());
246
247 // output rate in milliseconds (6 bytes)
248 payload[12..16].copy_from_slice(&0x30210001u32.to_le_bytes());
249 payload[16..18].copy_from_slice(&self.nav_period_ms.to_le_bytes());
250
251 // 1 data output per nav measurement (6 bytes)
252 payload[18..22].copy_from_slice(&0x30210002u32.to_le_bytes());
253 payload[22..24].copy_from_slice(&1u16.to_le_bytes());
254
255 // CFG-NAVSPG-DYNMODEL 8 = 4G Airborne (5 bytes)
256 payload[24..28].copy_from_slice(&0x20110021u32.to_le_bytes());
257 payload[28] = 8;
258
259 // CFG-NAVSPG-FIXMODE 3 = Auto 2/3D (5 bytes)
260 payload[29..33].copy_from_slice(&0x20110011u32.to_le_bytes());
261 payload[33] = 3u8;
262
263 self.tx(0x06, 0x8A, &payload).await
264 }
265 }
266 }
267
268 async fn cfg_rate(&mut self) -> bool {
269 let mut payload = [0u8; 6]; // #define CFG_RATE_LENGTH 6
270 payload[0..2].copy_from_slice(&self.nav_period_ms.to_le_bytes()); // meas rate
271 payload[2..4].copy_from_slice(&0x0001u16.to_le_bytes()); // nav rate = meas rate
272 payload[4..6].copy_from_slice(&0x0000u16.to_le_bytes()); // UTC time reference
273
274 self.tx(0x06, 0x08, &payload).await
275 }
276
277 async fn cfg_tp5(&mut self) -> bool {
278 let mut payload = [0u8; 32]; // #define SFG_TP5_LENGTH 32
279 payload[0] = 0; // Timepulse pin 0
280 payload[1] = 1; // Version 1
281 //payload[2] = 0; // reserved
282 //payload[3] = 0; // reserved
283 //payload[4..6].copy_from_slice(& 0u16.to_le_bytes()); // antenna delay
284 //payload[6..8].copy_from_slice(& 0u16.to_le_bytes()); // rf group delay
285 let pps_period_us = PPS_PERIOD_US; //(pps_period_ms as u32)*1000u32;
286 payload[8..12].copy_from_slice(&pps_period_us.to_le_bytes()); // pulse period
287 payload[12..16].copy_from_slice(&pps_period_us.to_le_bytes()); // pulse period if locked other set
288 let pulse_len_us = 1000u32;
289 payload[16..20].copy_from_slice(&pulse_len_us.to_le_bytes()); // pulse high time
290 payload[20..24].copy_from_slice(&pulse_len_us.to_le_bytes()); // pulse high time
291 // payload[24..28].copy_from_slice(& 0u32.to_le_bytes()); // pulse high time
292 payload[28..32].copy_from_slice(&0x01F7u32.to_le_bytes()); // pulse high time
293
294 self.tx(0x06, 0x31, &payload).await
295 }
296
297 async fn cfg_nav5(&mut self) -> bool {
298 let mut payload = [0u8; 36]; // #define CFG_NAV5_LENGTH 36
299 payload[0] = 5; // Parameters bitmask
300 payload[1] = 8; // Airbourne navigatin < 4G's
301 payload[2] = 3; // Auto 2d/3d fix mode
302 self.tx(0x06, 0x24, &payload).await
303 }
304
305 async fn cfg_msg(&mut self, class: u8, id: u8, decimation_rate: u8) -> bool {
306 let payload = [class, id, decimation_rate];
307 self.tx(0x06, 0x01, &payload).await
308 }
309
310 async fn look_for_ack(&mut self, ack: &[u8]) -> bool {
311 let mut buffer = [0u8; 256];
312
313 // Expected ack packet [0xB5u8, 0x62u8, 0x05u8, 0x01u8, 0x02u8, 0x00u8, 0x06u8, 0x00u8, 0x0Eu8, 0x37u8,]
314 // Read data block with 2 second timeout
315 match with_timeout(
316 Duration::from_secs(2),
317 self.uart.read_until_idle(&mut buffer),
318 )
319 .await
320 {
321 Ok(Ok(_size)) => {
322 for subarray in buffer.windows(ack.len()) {
323 if ack == subarray {
324 return true;
325 }
326 }
327 }
328 Ok(Err(_read_err)) => {
329 // underlying UART/read error from self.uart.read(...)
330 }
331 Err(_timeout_err) => {
332 // with_timeout timed out
333 }
334 }
335 false
336 }
337
338 async fn sync_baudrate(&mut self) -> bool {
339 // Determine baud rate
340 let bauds = [
341 Bitrate::Baud9600 as u32,
342 Bitrate::Baud38400 as u32,
343 Bitrate::Baud57600 as u32,
344 Bitrate::Baud115200 as u32,
345 Bitrate::Baud230400 as u32,
346 ]; //, 460800u32, 921600u32];
347
348 for _retries in 0..30 {
349 for baud in bauds {
350 // try baud rate
351 // set stm32 baud rate
352 let _result: Result<(), usart::ConfigError> = self.uart.set_baudrate(baud);
353
354 // set ublox the desired baud rate
355 let result = self.cfg_prt(self.baudrate as u32).await;
356
357 if result {
358 let _result: Result<(), usart::ConfigError> =
359 self.uart.set_baudrate(self.baudrate as u32);
360 return true;
361 }
362 }
363 }
364 false
365 }
366
367 pub async fn run(&mut self) {
368 let _synced = self.sync_baudrate().await;
369
370 // Disable these messages
371 self.cfg_msg(0x0A, 0x09, 0).await; // MON-HW
372 self.cfg_msg(0x0A, 0x0B, 0).await; // MON-HW2
373 self.cfg_msg(0x01, 0x04, 0).await; // NAV-DOP
374 self.cfg_msg(0x01, 0x03, 0).await; // NAV-STATUS
375 self.cfg_msg(0x01, 0x35, 0).await; // NAV-SAT
376 self.cfg_msg(0x01, 0x20, 0).await; // NAV-TIMEGPS
377 self.cfg_msg(0x01, 0x01, 0).await; // NAV-POSECEF (length 20)
378 self.cfg_msg(0x01, 0x11, 0).await; // NAV-VELECEF (length 20)
379
380 // These are needed if you want ECEF, but disable for now
381 self.cfg_msg(0x01, 0x20, 0).await; // NAV-TIMEGPS (length 16)
382 self.cfg_msg(0x01, 0x01, 0).await; // NAV-POSECEF (length 20)
383 self.cfg_msg(0x01, 0x11, 0).await; // NAV-VELECEF (length 20)
384
385 // Enable this messages
386 self.cfg_msg(0x01, 0x07, 1).await; // NAV-PVT (length 92)
387
388 // Set GPS Configuration (already done in cfg_prt() for UBX_M9)
389 if let Protocol::M8 = self.protocol {
390 self.cfg_rate().await;
391 self.cfg_tp5().await;
392 self.cfg_nav5().await;
393 }
394 pub struct UbxFrame {
395 cl: u8,
396 id: u8,
397 length: usize,
398 a: u8, // checksum
399 b: u8, // checksum
400 payload: [u8; UBX_MAX_PAYLOAD_BYTES],
401 }
402
403 let mut p: UbxFrame = UbxFrame {
404 cl: 0u8,
405 id: 0u8,
406 length: 0usize,
407 a: 0u8,
408 b: 0u8,
409 payload: [0u8; UBX_MAX_PAYLOAD_BYTES],
410 };
411 let mut n = 0usize;
412 let mut pps_timestamp = 0;
413 loop {
414 // get most recent pps timestamp
415 match pps::PPS_SIGNAL.try_take() {
416 Some(packet) => {
417 pps_timestamp = packet.header.timestamp;
418 }
419 None => {}
420 }
421
422 let mut buffer = [0u8; BUFFER_LEN];
423
424 let result = self.uart.read_until_idle(&mut buffer).await;
425 if let Ok(size) = result {
426 // This could be a function, but might as well just chug through this here
427 for &c in buffer[0..size].iter() {
428 // special case where we get 0xB5 randomly duplicated at the start (DMA wierdness).
429
430 if (c == 0xB5) && (n == 1) {
431 n = 0;
432 }
433
434 if n == 0
435 // header byte 1 "mu" character
436 {
437 if c == 0xB5 {
438 n += 1;
439 } else {
440 n = 0;
441 }
442 } else if n == 1
443 // header byte 2
444 {
445 if c == 0x62 {
446 n += 1;
447 } else if c == 0xB5 {
448 n = 1;
449 }
450 // repeated 'mu'
451 else {
452 n = 0;
453 }
454 } else if n == 2
455 // Class
456 {
457 p.a = 0;
458 p.b = 0; // Reset the checksum calculation
459 p.cl = c;
460 n += 1;
461 p.a = p.a.wrapping_add(c);
462 p.b = p.b.wrapping_add(p.a);
463 } else if n == 3
464 // ID, allow all
465 {
466 p.id = c;
467 n += 1;
468 p.a = p.a.wrapping_add(c);
469 p.b = p.b.wrapping_add(p.a);
470 } else if n == 4
471 // length LSB
472 {
473 p.length = c as usize;
474 n += 1;
475 p.a = p.a.wrapping_add(c);
476 p.b = p.b.wrapping_add(p.a);
477 } else if n == 5
478 // length MSB
479 {
480 p.length |= (c as usize) << 8;
481 if p.length > UBX_MAX_PAYLOAD_BYTES {
482 n = 0;
483 } else {
484 n += 1;
485 p.a = p.a.wrapping_add(c);
486 p.b = p.b.wrapping_add(p.a);
487 }
488 } else if n < p.length + 6
489 // Packet Payload bytes and first byte of checksum.
490 {
491 p.payload[n - 6] = c;
492 n += 1;
493 p.a = p.a.wrapping_add(c);
494 p.b = p.b.wrapping_add(p.a);
495 } else if n == p.length + 6
496 // Checksum A
497 {
498 if p.a != c {
499 n = 0;
500 } else {
501 n += 1;
502 }
503 } else {
504 n = 0;
505 if p.b == c {
506 // we found a valid packet
507 if (p.cl == 0x01) || (p.id == 0x07)
508 // pvt packet
509 {
510 let end_of_packet_timestamp = Instant::now();
511
512 // map the payload into the pvt union
513 let mut payload = [0u8; 92];
514 payload.copy_from_slice(&p.payload[0..92]);
515 let pvt = PvtUnion { payload };
516
517 // build up the device specific status register.
518 // (from Bitfield valid) [0] = validDate, [1] = validTime, [2] = fullyResolved, [3] validMag, none ignored
519 // (from Bitfield flags) [4] = gnssFixOK , [5] = diffSoln, [6] = psmState, [7] = headVehValid, [8] = carrSoln, none ignored
520 // (from Bitfield flags2) [9] = confirmedAvai, [10] = confirmedDate, [11] = confirmedTime, none ignored
521 // (from Bitfield flags3) [12] = invalidLlh, [13] = lastCorrectionAge
522
523 let mut status = 0u16;
524 let valid = unsafe { pvt.packet }.valid;
525 let flags = unsafe { pvt.packet }.flags;
526 let flags2 = unsafe { pvt.packet }.flags2;
527 let flags3 = unsafe { pvt.packet }.flags3;
528
529 if (valid & 0x01) != 0 {
530 status |= 0x0001
531 }; // validDate
532 if (valid & 0x02) != 0 {
533 status |= 0x0002
534 }; // validTime
535 if (valid & 0x04) != 0 {
536 status |= 0x0004
537 }; // fullyResolved
538 if (valid & 0x08) != 0 {
539 status |= 0x0008
540 }; // validMag
541
542 if (flags & 0x01) != 0 {
543 status |= 0x0010
544 }; // gnssFixOK
545 if (flags & 0x02) != 0 {
546 status |= 0x0020
547 }; // diffSoln
548 if (flags & 0x10) != 0 {
549 status |= 0x0020
550 }; // pmsState
551 if (flags & 0x20) != 0 {
552 status |= 0x0040
553 }; // headVehValid
554 if (flags & 0x80) != 0 {
555 status |= 0x0080
556 }; // carrSoln
557
558 if (flags2 & 0x20) != 0 {
559 status |= 0x0100
560 }; // confirmedAvai
561 if (flags2 & 0x40) != 0 {
562 status |= 0x0200
563 }; // confirmedDate
564 if (flags2 & 0x80) != 0 {
565 status |= 0x0400
566 }; // confirmedTime
567
568 if (flags3 & 0x0001) != 0 {
569 status |= 0x0800
570 }; // invalidLlh
571 if (flags3 & 0x0010) != 0 {
572 status |= 0x1000
573 }; // lastCorrectionAge
574
575 // put in terms of microseconds
576 let t0 = pps_timestamp as u64; // top of seconds
577 let t1 = end_of_packet_timestamp.as_micros();
578 let nav_dt = (self.nav_period_ms as u64) * 1000;
579
580 // phase offset from t0, doesn't matter if its a little old
581 // we are just counting off how many nav times we are behind the time pulse.
582 let dt = ((t1 - t0) / nav_dt) * nav_dt;
583
584 let timestamp = Instant::from_micros(t0 + dt);
585
586 let header = packets::RosflightPacketHeader {
587 timestamp: timestamp.as_micros(),
588 status,
589 };
590
591 let _fix_type =
592 packets::GNSSFixType::from_u8(unsafe { pvt.packet }.fix_type);
593
594 let _pi = 3.141592654;
595 let pvt_packet = packets::GNSSPacket {
596 header: header,
597 unix_seconds: unix_seconds_from_utc(
598 unsafe { pvt.packet }.year,
599 unsafe { pvt.packet }.month,
600 unsafe { pvt.packet }.day,
601 unsafe { pvt.packet }.hour,
602 unsafe { pvt.packet }.min,
603 unsafe { pvt.packet }.sec,
604 ),
605 unix_nanos: unsafe { pvt.packet }.nano,
606 // UBX NAV-PVT and ROSFLIGHT_GNSS both use decimal degrees.
607 lat: (unsafe { pvt.packet }.lat as f64) * 1.0e-7,
608 lon: (unsafe { pvt.packet }.lon as f64) * 1.0e-7,
609 height: (unsafe { pvt.packet }.height as f32) / 1000.0,
610 vel_n: (unsafe { pvt.packet }.vel_n as f32) / 1000.0,
611 vel_e: (unsafe { pvt.packet }.vel_e as f32) / 1000.0,
612 vel_d: (unsafe { pvt.packet }.vel_d as f32) / 1000.0,
613 h_acc: (unsafe { pvt.packet }.h_acc as f32) / 1000.0,
614 v_acc: (unsafe { pvt.packet }.v_acc as f32) / 1000.0,
615 s_acc: (unsafe { pvt.packet }.s_acc as f32) / 1000.0,
616 month: unsafe { pvt.packet }.month,
617 day: unsafe { pvt.packet }.day,
618 year: unsafe { pvt.packet }.year,
619 hour: unsafe { pvt.packet }.hour,
620 min: unsafe { pvt.packet }.min,
621 sec: unsafe { pvt.packet }.sec,
622 nano: unsafe { pvt.packet }.nano,
623 fix_type: packets::GNSSFixType::from_u8(
624 unsafe { pvt.packet }.fix_type,
625 ),
626 num_sats: unsafe { pvt.packet }.num_sv,
627 mag_dec: (unsafe { pvt.packet }.mag_dec as f32)
628 * 1.7453292519943296e-4,
629 time_correction: dt,
630 };
631 GNSS_SIGNAL.signal(Ok(pvt_packet));
632 }
633 }
634 }
635 }
636 }
637 }
638 }
639}
640
641#[embassy_executor::task]
642pub async fn task(mut ublox: UbloxSensor) {
643 ublox.run().await;
644}