sbp_msg_time.rs

use std::boxed::Box;
use std::error::Error;
use std::fs::File;
use std::io::BufReader;
use std::io::ErrorKind;
use std::result::Result;

use sbp::messages::SBP;
use sbp::messages;

use sbp::Error as SbpError;

fn maybe_time_aware(sbp: &SBP) -> Option<Box<dyn MessageTime>> {
    let msg_time: Option<Box<dyn MessageTime>> = match sbp.clone() {
        SBP::MsgExtEvent(m) => Some(Box::new(m)),
        SBP::MsgGPSTime(m) => Some(Box::new(m)),
        SBP::MsgPosLLH(m) => Some(Box::new(m)),
        _ => None,
    };
    msg_time
}

struct GpsTime {
    pub wn: u16,
    pub tow_s: f32,
}

trait MessageTime {
    fn gps_time(&self, last_full_time: &GpsTime) -> GpsTime;
}

impl MessageTime for messages::ext_events::MsgExtEvent {
    fn gps_time(&self, _: &GpsTime) -> GpsTime {
        let tow_ms = self.tow as f32;
        GpsTime{ wn: self.wn, tow_s: tow_ms / 1000.0 }
    }
}

impl MessageTime for messages::navigation::MsgGPSTime {
    fn gps_time(&self, _: &GpsTime) -> GpsTime {
        let tow_ms = self.tow as f32;
        GpsTime{ wn: self.wn, tow_s: tow_ms / 1000.0 }
    }
}

impl MessageTime for messages::navigation::MsgPosLLH {
    fn gps_time(&self, last_full_time: &GpsTime) -> GpsTime {
        let tow_ms = self.tow as f32;
        GpsTime{ wn: last_full_time.wn, tow_s: tow_ms / 1000.0 }
    }
}

fn main() -> Result<(), Box<dyn Error>> {
    let file = File::open("input.sbp",)?;
    let mut reader = BufReader::new(file);
    let mut parser = sbp::parser::Parser::new();
    let mut last_full_time = GpsTime { wn: 0, tow_s: 0.0 };
    loop {
        match parser.parse(&mut reader) {
            Ok(msg) => match msg {
                SBP::MsgLog(msg_log) => println!("{}", msg_log.text),
                other => {
                    let time_aware = maybe_time_aware(&other);
                    if let Some(time_aware) = time_aware {
                        let msg_generic = other.as_sbp_message();
                        let gps_time = time_aware.gps_time(&last_full_time);
                        println!("time aware message: msg_id={}, wn={}, tow_s={}", msg_generic.get_message_type(),
                                    gps_time.wn, gps_time.tow_s);
                        last_full_time = gps_time;
                    }
                }
            }
            Err(SbpError::IoError(err)) => {
                if err.kind() != ErrorKind::UnexpectedEof {
                    println!("Got an IO error: {:?}", err);
                }
                break;
            }
            Err(err) => { 
                println!("Got an unexpected error: {:?}", err);
                break;
            }
        }
    }
    Ok(())
}