#![no_std]
#![warn(missing_docs)]
use core::convert::TryFrom;
use core::ops::BitOr;
use core::slice::Iter;
pub(crate) mod common;
pub mod coords;
pub mod datetime;
pub(crate) mod gga;
pub(crate) mod gll;
pub(crate) mod modes;
pub(crate) mod rmc;
pub(crate) mod vtg;
pub use gga::GPSQuality;
pub use gga::GGA;
pub use gll::GLL;
pub use modes::Mode;
pub use rmc::RMC;
pub use vtg::VTG;
#[derive(Debug, Copy, Clone, PartialEq)]
pub enum Source {
GPS = 0b1,
GLONASS = 0b10,
Gallileo = 0b100,
Beidou = 0b1000,
GNSS = 0b10000,
}
pub struct SourceMask {
mask: u32,
}
impl SourceMask {
fn is_masked(&self, source: Source) -> bool {
source as u32 & self.mask == 0
}
}
impl Default for SourceMask {
fn default() -> Self {
SourceMask {
mask: u32::max_value(),
}
}
}
impl BitOr for Source {
type Output = SourceMask;
fn bitor(self, rhs: Self) -> Self::Output {
SourceMask {
mask: self as u32 | rhs as u32,
}
}
}
impl BitOr<Source> for SourceMask {
type Output = Self;
fn bitor(self, rhs: Source) -> Self {
SourceMask {
mask: self.mask | rhs as u32,
}
}
}
impl TryFrom<&str> for Source {
type Error = &'static str;
fn try_from(from: &str) -> Result<Self, Self::Error> {
match &from[0..2] {
"GP" => Ok(Source::GPS),
"GL" => Ok(Source::GLONASS),
"GA" => Ok(Source::Gallileo),
"BD" => Ok(Source::Beidou),
"GN" => Ok(Source::GNSS),
_ => Err("Source is not supported!"),
}
}
}
#[derive(Debug, Copy, Clone)]
pub enum Sentence {
RMC = 0b1,
VTG = 0b10,
GGA = 0b100,
GLL = 0b1000,
}
impl TryFrom<&str> for Sentence {
type Error = &'static str;
fn try_from(from: &str) -> Result<Self, Self::Error> {
match from {
"RMC" => Ok(Sentence::RMC),
"GGA" => Ok(Sentence::GGA),
"GLL" => Ok(Sentence::GLL),
"VTG" => Ok(Sentence::VTG),
_ => Err("Unsupported sentence type."),
}
}
}
pub struct SentenceMask {
mask: u32,
}
impl SentenceMask {
fn is_masked(&self, sentence: Sentence) -> bool {
sentence as u32 & self.mask == 0
}
}
impl Default for SentenceMask {
fn default() -> Self {
SentenceMask {
mask: u32::max_value(),
}
}
}
impl BitOr for Sentence {
type Output = SentenceMask;
fn bitor(self, rhs: Self) -> Self::Output {
SentenceMask {
mask: self as u32 | rhs as u32,
}
}
}
impl BitOr<Sentence> for SentenceMask {
type Output = Self;
fn bitor(self, rhs: Sentence) -> Self {
SentenceMask {
mask: self.mask | rhs as u32,
}
}
}
#[derive(Debug, PartialEq)]
pub enum ParseResult {
RMC(Option<RMC>),
GGA(Option<GGA>),
GLL(Option<GLL>),
VTG(Option<VTG>),
}
pub struct Parser {
buffer: [u8; 79],
buflen: usize,
chksum: u8,
expected_chksum: u8,
parser_state: ParserState,
source_mask: SourceMask,
sentence_mask: SentenceMask,
}
#[derive(Debug)]
enum ParserState {
WaitStart,
ReadUntilChkSum,
ChkSumUpper,
ChkSumLower,
WaitCR,
WaitLF,
}
struct ParserIterator<'a> {
parser: &'a mut Parser,
input: Iter<'a, u8>,
}
impl ParserIterator<'_> {
fn new<'a>(p: &'a mut Parser, inp: &'a [u8]) -> ParserIterator<'a> {
ParserIterator {
parser: p,
input: inp.iter(),
}
}
}
impl Iterator for ParserIterator<'_> {
type Item = Result<ParseResult, &'static str>;
fn next(&mut self) -> Option<Self::Item> {
while let Some(b) = self.input.next() {
let symbol = *b;
if let Some(r) = self.parser.parse_from_byte(symbol) {
return Some(r);
}
}
None
}
}
impl Parser {
pub fn new() -> Parser {
Parser {
buffer: [0u8; 79],
buflen: 0,
chksum: 0,
expected_chksum: 0,
parser_state: ParserState::WaitStart,
source_mask: Default::default(),
sentence_mask: Default::default(),
}
}
pub fn source_only(mut self, source: Source) -> Self {
self.source_mask = SourceMask {
mask: source as u32,
};
self
}
pub fn source_filter(mut self, source_mask: SourceMask) -> Self {
self.source_mask = source_mask;
self
}
pub fn sentence_only(mut self, sentence: Sentence) -> Self {
self.sentence_mask = SentenceMask {
mask: sentence as u32,
};
self
}
pub fn sentence_filter(mut self, sentence_mask: SentenceMask) -> Self {
self.sentence_mask = sentence_mask;
self
}
pub fn parse_from_bytes<'a>(
&'a mut self,
input: &'a [u8],
) -> impl Iterator<Item = Result<ParseResult, &'static str>> + 'a {
ParserIterator::new(self, input)
}
pub fn parse_from_byte(&mut self, symbol: u8) -> Option<Result<ParseResult, &'static str>> {
let (new_state, result) = match self.parser_state {
ParserState::WaitStart if symbol == b'$' => {
self.buflen = 0;
self.chksum = 0;
(ParserState::ReadUntilChkSum, None)
}
ParserState::WaitStart if symbol != b'$' => (ParserState::WaitStart, None),
ParserState::ReadUntilChkSum if symbol != b'*' => {
if self.buffer.len() < self.buflen {
(
ParserState::WaitStart,
Some(Err("NMEA sentence is too long!")),
)
} else {
self.buffer[self.buflen] = symbol;
self.buflen += 1;
self.chksum = self.chksum ^ symbol;
(ParserState::ReadUntilChkSum, None)
}
}
ParserState::ReadUntilChkSum if symbol == b'*' => (ParserState::ChkSumUpper, None),
ParserState::ChkSumUpper => match parse_hex_halfbyte(symbol) {
Ok(s) => {
self.expected_chksum = s;
(ParserState::ChkSumLower, None)
}
Err(e) => (ParserState::WaitStart, Some(Err(e))),
},
ParserState::ChkSumLower => match parse_hex_halfbyte(symbol) {
Ok(s) => {
if ((self.expected_chksum << 4) | s) != self.chksum {
(ParserState::WaitStart, Some(Err("Checksum error!")))
} else {
(ParserState::WaitCR, None)
}
}
Err(e) => (ParserState::WaitStart, Some(Err(e))),
},
ParserState::WaitCR if symbol == b'\r' => (ParserState::WaitLF, None),
ParserState::WaitLF if symbol == b'\n' => {
(ParserState::WaitStart, self.parse_sentence().transpose())
}
_ => (ParserState::WaitStart, Some(Err("NMEA format error!"))),
};
self.parser_state = new_state;
return result;
}
fn parse_sentence(&self) -> Result<Option<ParseResult>, &'static str> {
let input = from_ascii(&self.buffer[..self.buflen])?;
let mut iter = input.split(',');
let sentence_field = iter
.next()
.ok_or("Sentence type not found but mandatory!")?;
if sentence_field.len() < 5 {
return Err("Sentence field is too small. Must be 5 chars at least!");
}
let source = Source::try_from(sentence_field)?;
if self.source_mask.is_masked(source) {
return Ok(None);
}
let sentence = Sentence::try_from(&sentence_field[2..5])?;
if self.sentence_mask.is_masked(sentence) {
return Ok(None);
}
match sentence {
Sentence::RMC => Ok(Some(ParseResult::RMC(RMC::parse(source, &mut iter)?))),
Sentence::GGA => Ok(Some(ParseResult::GGA(GGA::parse(source, &mut iter)?))),
Sentence::GLL => Ok(Some(ParseResult::GLL(GLL::parse(source, &mut iter)?))),
Sentence::VTG => Ok(Some(ParseResult::VTG(VTG::parse(source, &mut iter)?))),
}
}
}
fn from_ascii(bytes: &[u8]) -> Result<&str, &'static str> {
if bytes.iter().all(|b| *b < 128) {
Ok(unsafe { core::str::from_utf8_unchecked(bytes) })
} else {
Err("Not an ascii!")
}
}
fn parse_hex_halfbyte(symbol: u8) -> Result<u8, &'static str> {
if symbol >= b'0' && symbol <= b'9' {
return Ok(symbol - b'0');
}
if symbol >= b'A' && symbol <= b'F' {
return Ok(symbol - b'A' + 10);
}
Err("Invalid HEX character.")
}
#[test]
fn test_source_bitor() {
let s = Source::GLONASS | Source::GPS | Source::Beidou;
assert!(s.mask == (Source::GLONASS as u32 | Source::GPS as u32 | Source::Beidou as u32));
}
#[test]
fn test_sentence_bitor() {
let s = Sentence::RMC | Sentence::VTG | Sentence::GGA;
assert!(s.mask == (Sentence::RMC as u32 | Sentence::VTG as u32 | Sentence::GGA as u32));
}
#[test]
fn test_create_filtered_parser() {
let _parser = Parser::new()
.source_filter(Source::GPS | Source::GLONASS)
.sentence_filter(Sentence::RMC | Sentence::GLL);
let _parser = Parser::new()
.source_only(Source::GPS)
.sentence_only(Sentence::RMC);
}