generated from luke-else/esp32-std-template
89 lines
3.1 KiB
Rust
89 lines
3.1 KiB
Rust
use std::sync::{Mutex, Arc};
|
|
use super::GpsError;
|
|
|
|
use nmea_parser;
|
|
use nmea_parser::gnss::FaaMode;
|
|
|
|
/// Data structure to store all relevant data collected from the gps
|
|
#[derive(Default)]
|
|
pub struct GpsData {
|
|
/// Latitude of reported GPS location
|
|
latitude: Option<f64>,
|
|
/// Longitude of reported GPS location
|
|
longitude: Option<f64>,
|
|
/// Calculated speed from GPS reciever
|
|
speed: Option<f64>,
|
|
/// Altitude reported from GPS location
|
|
altitude: Option<f64>,
|
|
}
|
|
|
|
impl GpsData {
|
|
pub fn to_string(&self) -> String {
|
|
format!("Latitude: {:.4?} \nLongitude: {:.4?} \nSpeed: {:.1?}mph \nAltitude: {:.1?}m",
|
|
self.latitude.unwrap_or_default(),
|
|
self.longitude.unwrap_or_default(),
|
|
self.speed.unwrap_or_default(),
|
|
self.altitude.unwrap_or_default()
|
|
)
|
|
}
|
|
|
|
/// Function to asynchronously go through and update GPS data from a nmea stream
|
|
pub async fn update(&mut self, buf: &Vec<u8>) -> Result<(), GpsError> {
|
|
let mut nmea_parser = nmea_parser::NmeaParser::new();
|
|
|
|
// Format the nmea buffer into a usable string
|
|
let nmea_raw = String::from_utf8(buf.to_owned())
|
|
.map_err(|_| GpsError::ReadError())?;
|
|
let nmea_vec: Vec<&str> = nmea_raw.split('$').collect();
|
|
|
|
// Loop through each sentence and use the information to update GPS data
|
|
for nmea_line in nmea_vec {
|
|
// Don't try and process / parse if the string is empty
|
|
if nmea_line.is_empty() {
|
|
continue;
|
|
}
|
|
|
|
// Construct string that is in the correct format for parsing
|
|
let mut sentence = "$".to_string();
|
|
sentence.push_str(nmea_line);
|
|
|
|
let nmea = match nmea_parser.parse_sentence(sentence.as_str()) {
|
|
Ok(nmea) => nmea,
|
|
// Don't continue processing a sentence if we know that it isn't supported
|
|
Err(_) => { continue; }
|
|
};
|
|
|
|
// print decoded gps data to serial
|
|
match nmea {
|
|
nmea_parser::ParsedMessage::Gga(gga) => {
|
|
self.latitude = gga.latitude;
|
|
self.longitude = gga.longitude;
|
|
self.altitude = gga.altitude;
|
|
}
|
|
nmea_parser::ParsedMessage::Gll(gll) => {
|
|
if gll.faa_mode.unwrap_or(FaaMode::NotValid) == FaaMode::Autonomous {
|
|
self.latitude = gll.latitude;
|
|
self.longitude = gll.longitude;
|
|
}
|
|
},
|
|
nmea_parser::ParsedMessage::Rmc(rms) => {
|
|
self.latitude = rms.latitude;
|
|
self.longitude = rms.longitude;
|
|
self.speed = Some(knots_to_mph(rms.sog_knots.unwrap_or_default()));
|
|
},
|
|
_ => {}
|
|
}
|
|
}
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub fn new() -> Arc<Mutex<GpsData>> {
|
|
return Arc::new(Mutex::new(GpsData::default()))
|
|
}
|
|
}
|
|
|
|
/// Function to simply convert knots to mph
|
|
fn knots_to_mph(knots: f64) -> f64{
|
|
knots * 1.150779
|
|
} |