106 lines
3.3 KiB
Rust
106 lines
3.3 KiB
Rust
//! u-blox helpers
|
|
#![allow(unused)]
|
|
|
|
use alloc::vec::Vec;
|
|
|
|
pub fn pretty_print_mon_ver(packet: &ublox::mon_ver::MonVerRef) -> alloc::string::String {
|
|
alloc::format!(
|
|
"UBX-MON-VER: SW version: {} HW version: {}; Extensions: {:?}",
|
|
packet.software_version(),
|
|
packet.hardware_version(),
|
|
packet.extension().collect::<Vec<&str>>()
|
|
)
|
|
}
|
|
|
|
pub fn pretty_print_mon_comms(packet: &ublox::mon_comms::MonCommsRef) -> alloc::string::String {
|
|
alloc::format!(
|
|
"UBX-MON-COMMS: {} ports ({:?}), {} tx err, info {:?}",
|
|
packet.n_ports(),
|
|
packet.prot_ids(),
|
|
packet.tx_errors(),
|
|
packet
|
|
.ports()
|
|
.map(|v| alloc::format!("{v:?}"))
|
|
.collect::<Vec<_>>()
|
|
)
|
|
}
|
|
|
|
pub fn pretty_print_mon_rf(packet: &ublox::mon_rf::MonRfRef) -> alloc::string::String {
|
|
alloc::format!(
|
|
"UBX-MON-RF: {:?}",
|
|
packet
|
|
.blocks()
|
|
.map(|v| alloc::format!("{v:?}"))
|
|
.collect::<Vec<_>>()
|
|
)
|
|
}
|
|
|
|
pub fn pretty_print_nav_sat(packet: &ublox::nav_sat::NavSatRef) -> alloc::string::String {
|
|
// sort by quality flags
|
|
let mut sat_infos = packet.svs().collect::<Vec<_>>();
|
|
sat_infos.sort_unstable_by_key(|v| v.flags_raw() & 0x7);
|
|
sat_infos.reverse();
|
|
|
|
let sat_info_strings = sat_infos
|
|
.iter()
|
|
.map(|v| {
|
|
alloc::format!(
|
|
"- {}/{:3}: E {:3.0}, A {:3.0}, CNO {:2.1} dBHz, {:?}, {:?}",
|
|
v.gnss_id(),
|
|
v.sv_id(),
|
|
v.elev(),
|
|
v.azim(),
|
|
v.cno(),
|
|
v.flags().quality_ind(),
|
|
v.flags().health(),
|
|
)
|
|
})
|
|
.collect::<Vec<_>>();
|
|
|
|
alloc::format!(
|
|
"UBX-NAV-SAT: num {:?}\n{}",
|
|
packet.num_svs(),
|
|
sat_info_strings.join("\n")
|
|
)
|
|
}
|
|
|
|
impl<'a> TryFrom<&ublox::nav_pvt::proto27::NavPvtRef<'a>> for crate::applogic::GnssFix {
|
|
type Error = alloc::string::String;
|
|
|
|
fn try_from(value: &ublox::nav_pvt::proto27::NavPvtRef<'a>) -> Result<Self, Self::Error> {
|
|
let has_time = value.fix_type() == ublox::GnssFixType::Fix3D
|
|
|| value.fix_type() == ublox::GnssFixType::GPSPlusDeadReckoning
|
|
|| value.fix_type() == ublox::GnssFixType::TimeOnlyFix;
|
|
let has_posvel = value.fix_type() == ublox::GnssFixType::Fix3D
|
|
|| value.fix_type() == ublox::GnssFixType::GPSPlusDeadReckoning;
|
|
|
|
if !has_posvel || !has_time {
|
|
return Err(alloc::format!(
|
|
"NAV-PVT without valid fix: {:?}",
|
|
value.fix_type()
|
|
));
|
|
}
|
|
|
|
let pos: ublox::PositionLLA = value.into();
|
|
let vel: ublox::Velocity = value.into();
|
|
|
|
let time: chrono::DateTime<chrono::Utc> = value
|
|
.try_into()
|
|
.map_err(|err| alloc::format!("Failed to convert time: {err}"))?;
|
|
|
|
let latitude_deg = pos.lat;
|
|
let longitude_deg = pos.lon;
|
|
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
|
let heading_deg = vel.heading as f32;
|
|
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
|
let speed_mps = vel.speed as f32;
|
|
|
|
Ok(Self {
|
|
time: Some(time.naive_utc()),
|
|
latitude_deg,
|
|
longitude_deg,
|
|
heading_deg: Some(heading_deg),
|
|
speed_mps: Some(speed_mps),
|
|
})
|
|
}
|
|
}
|