use super::{
ubx_checksum, MemWriter, Position, UbxChecksumCalc, UbxPacketCreator, UbxPacketMeta,
UbxUnknownPacketRef, SYNC_CHAR_1, SYNC_CHAR_2,
};
use crate::error::{MemWriterError, ParserError};
use bitflags::bitflags;
use chrono::prelude::*;
use core::fmt;
use num_traits::cast::{FromPrimitive, ToPrimitive};
use num_traits::float::FloatCore;
use ublox_derive::{
define_recv_packets, ubx_extend, ubx_extend_bitflags, ubx_packet_recv, ubx_packet_recv_send,
ubx_packet_send,
};
#[ubx_packet_recv]
#[ubx(class = 1, id = 2, fixed_payload_len = 28)]
struct NavPosLlh {
itow: u32,
#[ubx(map_type = f64, scale = 1e-7, alias = lon_degrees)]
lon: i32,
#[ubx(map_type = f64, scale = 1e-7, alias = lat_degrees)]
lat: i32,
#[ubx(map_type = f64, scale = 1e-3)]
height_meters: i32,
#[ubx(map_type = f64, scale = 1e-3)]
height_msl: i32,
#[ubx(map_type = f64, scale = 1e-3)]
h_ack: u32,
#[ubx(map_type = f64, scale = 1e-3)]
v_acc: u32,
}
#[ubx_packet_recv]
#[ubx(class = 1, id = 0x12, fixed_payload_len = 36)]
struct NavVelNed {
itow: u32,
#[ubx(map_type = f64, scale = 1e-2)]
vel_north: i32,
#[ubx(map_type = f64, scale = 1e-2)]
vel_east: i32,
#[ubx(map_type = f64, scale = 1e-2)]
vel_down: i32,
#[ubx(map_type = f64, scale = 1e-2)]
speed_3d: u32,
#[ubx(map_type = f64, scale = 1e-2)]
ground_speed: u32,
#[ubx(map_type = f64, scale = 1e-5, alias = heading_degrees)]
heading: i32,
#[ubx(map_type = f64, scale = 1e-2)]
speed_accuracy_estimate: u32,
#[ubx(map_type = f64, scale = 1e-5)]
course_heading_accuracy_estimate: u32,
}
#[ubx_packet_recv]
#[ubx(class = 1, id = 0x07, fixed_payload_len = 92)]
struct NavPosVelTime {
itow: u32,
year: u16,
month: u8,
day: u8,
hour: u8,
min: u8,
sec: u8,
valid: u8,
time_accuracy: u32,
nanosecond: i32,
#[ubx(map_type = GpsFix)]
fix_type: u8,
#[ubx(map_type = NavPosVelTimeFlags)]
flags: u8,
#[ubx(map_type = NavPosVelTimeFlags2)]
flags2: u8,
num_satellites: u8,
#[ubx(map_type = f64, scale = 1e-7, alias = lon_degrees)]
lon: i32,
#[ubx(map_type = f64, scale = 1e-7, alias = lat_degrees)]
lat: i32,
#[ubx(map_type = f64, scale = 1e-3)]
height_meters: i32,
#[ubx(map_type = f64, scale = 1e-3)]
height_msl: i32,
horiz_accuracy: u32,
vert_accuracy: u32,
#[ubx(map_type = f64, scale = 1e-3)]
vel_north: i32,
#[ubx(map_type = f64, scale = 1e-3)]
vel_east: i32,
#[ubx(map_type = f64, scale = 1e-3)]
vel_down: i32,
#[ubx(map_type = f64, scale = 1e-3)]
ground_speed: u32,
#[ubx(map_type = f64, scale = 1e-5, alias = heading_degrees)]
heading: i32,
#[ubx(map_type = f64, scale = 1e-3)]
speed_accuracy_estimate: u32,
#[ubx(map_type = f64, scale = 1e-5)]
heading_accuracy_estimate: u32,
pdop: u16,
reserved1: [u8; 6],
#[ubx(map_type = f64, scale = 1e-5, alias = heading_of_vehicle_degrees)]
heading_of_vehicle: i32,
#[ubx(map_type = f64, scale = 1e-2, alias = magnetic_declination_degrees)]
magnetic_declination: i16,
#[ubx(map_type = f64, scale = 1e-2, alias = magnetic_declination_accuracy_degrees)]
magnetic_declination_accuracy: u16,
}
#[ubx_extend_bitflags]
#[ubx(from, rest_reserved)]
bitflags! {
pub struct NavPosVelTimeFlags: u8 {
const GPS_FIX_OK = 1;
const DIFF_SOLN = 2;
const HEAD_VEH_VALID = 0x20;
const CARR_SOLN_FLOAT = 0x40;
const CARR_SOLN_FIXED = 0x80;
}
}
#[ubx_extend_bitflags]
#[ubx(from, rest_reserved)]
bitflags! {
pub struct NavPosVelTimeFlags2: u8 {
const CONFIRMED_AVAI = 0x20;
const CONFIRMED_DATE = 0x40;
const CONFIRMED_TIME = 0x80;
}
}
#[ubx_packet_recv]
#[ubx(class = 1, id = 3, fixed_payload_len = 16)]
struct NavStatus {
itow: u32,
#[ubx(map_type = GpsFix)]
fix_type: u8,
#[ubx(map_type = NavStatusFlags)]
flags: u8,
#[ubx(map_type = FixStatusInfo)]
fix_stat: u8,
#[ubx(map_type = NavStatusFlags2)]
flags2: u8,
time_to_first_fix: u32,
uptime_ms: u32,
}
#[ubx_packet_recv]
#[ubx(class = 1, id = 4, fixed_payload_len = 18)]
struct NavDop {
itow: u32,
#[ubx(map_type = f32, scale = 1e-2)]
geometric_dop: u16,
#[ubx(map_type = f32, scale = 1e-2)]
position_dop: u16,
#[ubx(map_type = f32, scale = 1e-2)]
time_dop: u16,
#[ubx(map_type = f32, scale = 1e-2)]
vertical_dop: u16,
#[ubx(map_type = f32, scale = 1e-2)]
horizontal_dop: u16,
#[ubx(map_type = f32, scale = 1e-2)]
northing_dop: u16,
#[ubx(map_type = f32, scale = 1e-2)]
easting_dop: u16,
}
#[ubx_packet_recv]
#[ubx(class = 1, id = 6, fixed_payload_len = 52)]
struct NavSolution {
itow: u32,
ftow_ns: i32,
week: i16,
#[ubx(map_type = GpsFix)]
fix_type: u8,
#[ubx(map_type = NavStatusFlags)]
flags: u8,
#[ubx(map_type = f64, scale = 1e-2)]
ecef_x: i32,
#[ubx(map_type = f64, scale = 1e-2)]
ecef_y: i32,
#[ubx(map_type = f64, scale = 1e-2)]
ecef_z: i32,
#[ubx(map_type = f64, scale = 1e-2)]
position_accuracy_estimate: u32,
#[ubx(map_type = f64, scale = 1e-2)]
ecef_vx: i32,
#[ubx(map_type = f64, scale = 1e-2)]
ecef_vy: i32,
#[ubx(map_type = f64, scale = 1e-2)]
ecef_vz: i32,
#[ubx(map_type = f64, scale = 1e-2)]
speed_accuracy_estimate: u32,
#[ubx(map_type = f32, scale = 1e-2)]
pdop: u16,
reserved1: u8,
num_sv: u8,
reserved2: [u8; 4],
}
#[ubx_extend]
#[ubx(from, rest_reserved)]
#[repr(u8)]
#[derive(Debug, Copy, Clone, PartialEq)]
pub enum GpsFix {
NoFix = 0,
DeadReckoningOnly = 1,
Fix2D = 2,
Fix3D = 3,
GPSPlusDeadReckoning = 4,
TimeOnlyFix = 5,
}
#[ubx_extend_bitflags]
#[ubx(from, rest_reserved)]
bitflags! {
pub struct NavStatusFlags: u8 {
const GPS_FIX_OK = 1;
const DIFF_SOLN = 2;
const WKN_SET = 4;
const TOW_SET = 8;
}
}
#[repr(transparent)]
#[derive(Copy, Clone)]
pub struct FixStatusInfo(u8);
impl FixStatusInfo {
pub const fn has_pr_prr_correction(self) -> bool {
(self.0 & 1) == 1
}
pub fn map_matching(self) -> MapMatchingStatus {
let bits = (self.0 >> 6) & 3;
match bits {
0 => MapMatchingStatus::None,
1 => MapMatchingStatus::Valid,
2 => MapMatchingStatus::Used,
3 => MapMatchingStatus::Dr,
_ => unreachable!(),
}
}
pub const fn from(x: u8) -> Self {
Self(x)
}
}
impl fmt::Debug for FixStatusInfo {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
f.debug_struct("FixStatusInfo")
.field("has_pr_prr_correction", &self.has_pr_prr_correction())
.field("map_matching", &self.map_matching())
.finish()
}
}
#[derive(Copy, Clone, Debug)]
pub enum MapMatchingStatus {
None = 0,
Valid = 1,
Used = 2,
Dr = 3,
}
#[ubx_extend]
#[ubx(from, rest_reserved)]
#[repr(u8)]
#[derive(Debug, Copy, Clone)]
enum NavStatusFlags2 {
Acquisition = 0,
Tracking = 1,
PowerOptimizedTracking = 2,
Inactive = 3,
}
#[ubx_packet_send]
#[ubx(
class = 0x0B,
id = 0x01,
fixed_payload_len = 48,
flags = "default_for_builder"
)]
struct AidIni {
ecef_x_or_lat: i32,
ecef_y_or_lon: i32,
ecef_z_or_alt: i32,
pos_accuracy: u32,
time_cfg: u16,
week_or_ym: u16,
tow_or_hms: u32,
tow_ns: i32,
tm_accuracy_ms: u32,
tm_accuracy_ns: u32,
clk_drift_or_freq: i32,
clk_drift_or_freq_accuracy: u32,
flags: u32,
}
impl AidIniBuilder {
pub fn set_position(mut self, pos: Position) -> Self {
self.ecef_x_or_lat = (pos.lat * 10_000_000.0) as i32;
self.ecef_y_or_lon = (pos.lon * 10_000_000.0) as i32;
self.ecef_z_or_alt = (pos.alt * 100.0) as i32;
self.flags |= (1 << 0) | (1 << 5);
self
}
pub fn set_time(mut self, tm: DateTime<Utc>) -> Self {
self.week_or_ym = (match tm.year_ce() {
(true, yr) => yr - 2000,
(false, _) => {
panic!("AID-INI packet only supports years after 2000");
}
} * 100
+ tm.month0()) as u16;
self.tow_or_hms = tm.hour() * 10000 + tm.minute() * 100 + tm.second();
self.tow_ns = tm.nanosecond() as i32;
self.flags |= (1 << 1) | (1 << 10);
self
}
}
#[ubx_packet_recv]
#[ubx(class = 0x0B, id = 0x32, fixed_payload_len = 16)]
struct AlpSrv {
pub id_size: u8,
pub data_type: u8,
pub offset: u16,
pub size: u16,
pub file_id: u16,
pub data_size: u16,
pub id1: u8,
pub id2: u8,
pub id3: u32,
}
#[ubx_packet_recv]
#[ubx(class = 5, id = 1, fixed_payload_len = 2)]
struct AckAck {
class: u8,
msg_id: u8,
}
impl<'a> AckAckRef<'a> {
pub fn is_ack_for<T: UbxPacketMeta>(&self) -> bool {
self.class() == T::CLASS && self.msg_id() == T::ID
}
}
#[ubx_packet_recv]
#[ubx(class = 5, id = 0, fixed_payload_len = 2)]
struct AckNak {
class: u8,
msg_id: u8,
}
impl<'a> AckNakRef<'a> {
pub fn is_nak_for<T: UbxPacketMeta>(&self) -> bool {
self.class() == T::CLASS && self.msg_id() == T::ID
}
}
#[ubx_packet_send]
#[ubx(class = 6, id = 4, fixed_payload_len = 4)]
struct CfgRst {
#[ubx(map_type = NavBbrMask)]
nav_bbr_mask: u16,
#[ubx(map_type = ResetMode)]
reset_mode: u8,
reserved1: u8,
}
#[ubx_extend_bitflags]
#[ubx(into_raw, rest_reserved)]
bitflags! {
pub struct NavBbrMask: u16 {
const EPHEMERIS = 1;
const ALMANACH = 2;
const HEALTH = 4;
const KLOBUCHARD = 8;
const POSITION = 16;
const CLOCK_DRIFT = 32;
const OSCILATOR_PARAMETER = 64;
const UTC_CORRECTION_PARAMETERS = 0x80;
const RTC = 0x100;
const SFDR_PARAMETERS = 0x800;
const SFDR_VEHICLE_MONITORING_PARAMETERS = 0x1000;
const TCT_PARAMETERS = 0x2000;
const AUTONOMOUS_ORBIT_PARAMETERS = 0x8000;
}
}
#[derive(Clone, Copy)]
#[repr(transparent)]
pub struct NavBbrPredefinedMask(u16);
impl From<NavBbrPredefinedMask> for NavBbrMask {
fn from(x: NavBbrPredefinedMask) -> Self {
Self::from_bits_truncate(x.0)
}
}
impl NavBbrPredefinedMask {
pub const HOT_START: NavBbrPredefinedMask = NavBbrPredefinedMask(0);
pub const WARM_START: NavBbrPredefinedMask = NavBbrPredefinedMask(1);
pub const COLD_START: NavBbrPredefinedMask = NavBbrPredefinedMask(0xFFFF);
}
#[repr(u8)]
#[derive(Clone, Copy, Debug)]
pub enum ResetMode {
HardwareResetImmediately = 0,
ControlledSoftwareReset = 0x1,
ControlledSoftwareResetGpsOnly = 0x02,
HardwareResetAfterShutdown = 0x04,
ControlledGpsStop = 0x08,
ControlledGpsStart = 0x09,
}
impl ResetMode {
const fn into_raw(self) -> u8 {
self as u8
}
}
#[ubx_packet_recv_send]
#[ubx(class = 0x06, id = 0x00, fixed_payload_len = 20)]
struct CfgPrtUart {
#[ubx(map_type = UartPortId, may_fail)]
portid: u8,
reserved0: u8,
tx_ready: u16,
mode: u32,
baud_rate: u32,
in_proto_mask: u16,
out_proto_mask: u16,
flags: u16,
reserved5: u16,
}
#[ubx_extend]
#[ubx(from_unchecked, into_raw, rest_error)]
#[repr(u8)]
#[derive(Debug, Copy, Clone)]
pub enum UartPortId {
Uart1 = 1,
Uart2 = 2,
}
#[ubx_packet_recv_send]
#[ubx(
class = 0x06,
id = 0x00,
fixed_payload_len = 20,
flags = "default_for_builder"
)]
struct CfgPrtSpi {
#[ubx(map_type = SpiPortId, may_fail)]
portid: u8,
reserved0: u8,
tx_ready: u16,
mode: u32,
reserved3: u32,
#[ubx(map_type = InProtoMask)]
in_proto_mask: u16,
#[ubx(map_type = OutProtoMask)]
out_proto_mask: u16,
flags: u16,
reserved5: u16,
}
#[ubx_extend_bitflags]
#[ubx(from, into_raw, rest_reserved)]
bitflags! {
#[derive(Default)]
pub struct InProtoMask: u16 {
const UBOX = 1;
const NMEA = 2;
const RTCM = 4;
const RTCM3 = 0x20;
}
}
#[ubx_extend_bitflags]
#[ubx(from, into_raw, rest_reserved)]
bitflags! {
#[derive(Default)]
pub struct OutProtoMask: u16 {
const UBOX = 1;
const NMEA = 2;
const RTCM3 = 0x20;
}
}
#[ubx_extend]
#[ubx(from_unchecked, into_raw, rest_error)]
#[repr(u8)]
#[derive(Debug, Copy, Clone)]
pub enum SpiPortId {
Spi = 4,
}
impl Default for SpiPortId {
fn default() -> Self {
Self::Spi
}
}
#[ubx_packet_recv]
#[ubx(class = 1, id = 0x21, fixed_payload_len = 20)]
struct NavTimeUTC {
itow: u32,
time_accuracy_estimate_ns: u32,
nanos: i32,
year: u16,
month: u8,
day: u8,
hour: u8,
min: u8,
sec: u8,
#[ubx(map_type = NavTimeUtcFlags)]
valid: u8,
}
#[ubx_extend_bitflags]
#[ubx(from, rest_reserved)]
bitflags! {
pub struct NavTimeUtcFlags: u8 {
const VALID_TOW = 1;
const VALID_WKN = 2;
const VALID_UTC = 4;
}
}
#[ubx_packet_send]
#[ubx(class = 6, id = 8, fixed_payload_len = 6)]
struct CfgRate {
measure_rate_ms: u16,
nav_rate: u16,
#[ubx(map_type = AlignmentToReferenceTime)]
time_ref: u16,
}
#[repr(u16)]
#[derive(Clone, Copy, Debug)]
pub enum AlignmentToReferenceTime {
Utc = 0,
Gps = 1,
}
impl AlignmentToReferenceTime {
const fn into_raw(self) -> u16 {
self as u16
}
}
#[ubx_packet_send]
#[ubx(class = 6, id = 1, fixed_payload_len = 3)]
struct CfgMsgSinglePort {
msg_class: u8,
msg_id: u8,
rate: u8,
}
impl CfgMsgSinglePortBuilder {
#[inline]
pub fn set_rate_for<T: UbxPacketMeta>(rate: u8) -> Self {
Self {
msg_class: T::CLASS,
msg_id: T::ID,
rate,
}
}
}
#[ubx_packet_send]
#[ubx(class = 6, id = 1, fixed_payload_len = 8)]
struct CfgMsgAllPorts {
msg_class: u8,
msg_id: u8,
rates: [u8; 6],
}
impl CfgMsgAllPortsBuilder {
#[inline]
pub fn set_rate_for<T: UbxPacketMeta>(rates: [u8; 6]) -> Self {
Self {
msg_class: T::CLASS,
msg_id: T::ID,
rates,
}
}
}
#[ubx_packet_recv_send]
#[ubx(
class = 0x06,
id = 0x24,
fixed_payload_len = 36,
flags = "default_for_builder"
)]
struct CfgNav5 {
#[ubx(map_type = CfgNav5Params)]
mask: u16,
#[ubx(map_type = CfgNav5DynModel, may_fail)]
dyn_model: u8,
#[ubx(map_type = CfgNav5FixMode, may_fail)]
fix_mode: u8,
#[ubx(map_type = f64, scale = 0.01)]
fixed_alt: i32,
#[ubx(map_type = f64, scale = 0.0001)]
fixed_alt_var: u32,
min_elev_degrees: i8,
dr_limit: u8,
#[ubx(map_type = f32, scale = 0.1)]
pdop: u16,
#[ubx(map_type = f32, scale = 0.1)]
tdop: u16,
pacc: u16,
tacc: u16,
#[ubx(map_type = f32, scale = 0.01)]
static_hold_thresh: u8,
dgps_time_out: u8,
cno_thresh_num_svs: u8,
cno_thresh: u8,
reserved1: [u8; 2],
static_hold_max_dist: u16,
#[ubx(map_type = CfgNav5UtcStandard, may_fail)]
utc_standard: u8,
reserved2: [u8; 5],
}
#[ubx_extend_bitflags]
#[ubx(from, into_raw, rest_reserved)]
bitflags! {
#[derive(Default)]
pub struct CfgNav5Params: u16 {
const DYN = 1;
const MIN_EL = 2;
const POS_FIX_MODE = 4;
const DR_LIM = 8;
const POS_MASK_APPLY = 0x10;
const TIME_MASK = 0x20;
const STATIC_HOLD_MASK = 0x40;
const DGPS_MASK = 0x80;
const CNO_THRESHOLD = 0x100;
const UTC = 0x400;
}
}
#[ubx_extend]
#[ubx(from_unchecked, into_raw, rest_error)]
#[repr(u8)]
#[derive(Debug, Copy, Clone, PartialEq)]
pub enum CfgNav5DynModel {
Portable = 0,
Stationary = 2,
Pedestrian = 3,
Automotive = 4,
Sea = 5,
AirborneWithLess1gAcceleration = 6,
AirborneWithLess2gAcceleration = 7,
AirborneWith4gAcceleration = 8,
WristWornWatch = 9,
Bike = 10,
}
impl Default for CfgNav5DynModel {
fn default() -> Self {
Self::AirborneWith4gAcceleration
}
}
#[ubx_extend]
#[ubx(from_unchecked, into_raw, rest_error)]
#[repr(u8)]
#[derive(Debug, Copy, Clone, PartialEq)]
pub enum CfgNav5FixMode {
Only2D = 1,
Only3D = 2,
Auto2D3D = 3,
}
impl Default for CfgNav5FixMode {
fn default() -> Self {
CfgNav5FixMode::Auto2D3D
}
}
#[ubx_extend]
#[ubx(from_unchecked, into_raw, rest_error)]
#[repr(u8)]
#[derive(Debug, Copy, Clone, PartialEq)]
pub enum CfgNav5UtcStandard {
Automatic = 0,
Usno = 3,
UtcSu = 6,
UtcChina = 7,
}
impl Default for CfgNav5UtcStandard {
fn default() -> Self {
Self::Automatic
}
}
#[derive(Clone, Copy)]
#[repr(transparent)]
struct ScaleBack<T: FloatCore + FromPrimitive + ToPrimitive>(T);
impl<T: FloatCore + FromPrimitive + ToPrimitive> ScaleBack<T> {
fn as_i32(self, x: T) -> i32 {
let x = (x * self.0).round();
if x < T::from_i32(i32::min_value()).unwrap() {
i32::min_value()
} else if x > T::from_i32(i32::max_value()).unwrap() {
i32::max_value()
} else {
x.to_i32().unwrap()
}
}
fn as_u32(self, x: T) -> u32 {
let x = (x * self.0).round();
if !x.is_sign_negative() {
if x <= T::from_u32(u32::max_value()).unwrap() {
x.to_u32().unwrap()
} else {
u32::max_value()
}
} else {
0
}
}
fn as_u16(self, x: T) -> u16 {
let x = (x * self.0).round();
if !x.is_sign_negative() {
if x <= T::from_u16(u16::max_value()).unwrap() {
x.to_u16().unwrap()
} else {
u16::max_value()
}
} else {
0
}
}
fn as_u8(self, x: T) -> u8 {
let x = (x * self.0).round();
if !x.is_sign_negative() {
if x <= T::from_u8(u8::max_value()).unwrap() {
x.to_u8().unwrap()
} else {
u8::max_value()
}
} else {
0
}
}
}
#[ubx_packet_recv]
#[ubx(class = 0x0a, id = 0x04, max_payload_len = 1240)]
struct MonVer {
#[ubx(map_type = &str, may_fail, from = mon_ver::convert_to_str_unchecked,
is_valid = mon_ver::is_cstr_valid, get_as_ref)]
software_version: [u8; 30],
#[ubx(map_type = &str, may_fail, from = mon_ver::convert_to_str_unchecked,
is_valid = mon_ver::is_cstr_valid, get_as_ref)]
hardware_version: [u8; 10],
#[ubx(map_type = impl Iterator<Item=&str>, may_fail,
from = mon_ver::extension_to_iter,
is_valid = mon_ver::is_extension_valid)]
extension: [u8; 0],
}
mod mon_ver {
pub(crate) fn convert_to_str_unchecked(bytes: &[u8]) -> &str {
let null_pos = bytes
.iter()
.position(|x| *x == 0)
.expect("is_cstr_valid bug?");
core::str::from_utf8(&bytes[0..=null_pos])
.expect("is_cstr_valid should have prevented this code from running")
}
pub(crate) fn is_cstr_valid(bytes: &[u8]) -> bool {
let null_pos = match bytes.iter().position(|x| *x == 0) {
Some(pos) => pos,
None => {
return false;
}
};
core::str::from_utf8(&bytes[0..=null_pos]).is_ok()
}
pub(crate) fn is_extension_valid(payload: &[u8]) -> bool {
if payload.len() % 30 == 0 {
for chunk in payload.chunks(30) {
if !is_cstr_valid(chunk) {
return false;
}
}
true
} else {
false
}
}
pub(crate) fn extension_to_iter(payload: &[u8]) -> impl Iterator<Item = &str> {
payload.chunks(30).map(|x| convert_to_str_unchecked(x))
}
}
define_recv_packets!(
enum PacketRef {
_ = UbxUnknownPacketRef,
NavPosLlh,
NavStatus,
NavDop,
NavPosVelTime,
NavSolution,
NavVelNed,
NavTimeUTC,
AlpSrv,
AckAck,
AckNak,
CfgPrtSpi,
CfgPrtUart,
CfgNav5,
MonVer
}
);