From 23ff182b651f753685d60616a6efc0fe7e4c2310 Mon Sep 17 00:00:00 2001
From: Vladan Popovic <vladanovic@gmail.com>
Date: Wed, 20 Jul 2022 12:33:11 +0200
Subject: [PATCH] use same interface in gps as in gsm modem

---
 src/gps.rs | 58 ++++++++++++++++++++++++++++++++----------------------
 1 file changed, 34 insertions(+), 24 deletions(-)

diff --git a/src/gps.rs b/src/gps.rs
index 717c605..ef3f90a 100644
--- a/src/gps.rs
+++ b/src/gps.rs
@@ -8,24 +8,22 @@ use std::{
 };
 
 use esp_idf_hal::prelude::*;
-use esp_idf_hal::serial;
-use esp_idf_hal::gpio::*;
+use esp_idf_hal::serial::{self, Rx, Tx};
 
 use ublox::*;
 
 use crate::modem::Msg;
 use crate::serial::SerialIO;
 
-struct Device<UART: serial::Uart> {
+struct GpsModule<UART: serial::Uart> {
     port: SerialIO<UART>,
     parser: Parser<Vec<u8>>,
 }
 
-impl<UART: serial::Uart> Device<UART> {
-    pub fn new<T: Sync + Send>(port: serial::Serial<UART, Gpio12<T>, Gpio13<T>>) -> Device<UART> {
+impl<UART: serial::Uart> GpsModule<UART> {
+    pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
         let parser = Parser::default();
-        let (tx, rx) = port.split();
-        Device { port: SerialIO::new(tx, rx), parser }
+        GpsModule { port: SerialIO::new(tx, rx), parser }
     }
 
     pub fn write_all(&mut self, data: &[u8]) -> std::io::Result<()> {
@@ -39,6 +37,7 @@ impl<UART: serial::Uart> Device<UART> {
             let mut local_buf = [0; 1024];
             let nbytes = self.read_port(&mut local_buf)?;
             if nbytes == 0 {
+                println!("no bytes to read :(");
                 break;
             }
             // parser.consume adds the buffer to its internal buffer, and
@@ -52,7 +51,7 @@ impl<UART: serial::Uart> Device<UART> {
         Ok(())
     }
 
-    pub fn wait_for_ack<T: UbxPacketMeta>(&mut self, timeout: Duration) -> std::io::Result<()> {
+    pub fn wait_for_ack<T: UbxPacketMeta>(&mut self, timeout: Duration) -> std::io::Result<bool> {
         let mut found_packet = false;
         println!("LOOKING FOR ACK ...");
         let start = Instant::now();
@@ -64,9 +63,13 @@ impl<UART: serial::Uart> Device<UART> {
                         found_packet = true;
                     }
                 }
+                else if let PacketRef::AckNak(nak) = packet {
+                    println!("NAK PACKET: {} {}", nak.class(), nak.msg_id());
+                }
             })?;
         }
-        Ok(())
+        println!("exiting wait_for_ack");
+        Ok(found_packet)
     }
 
     /// Reads the serial port, converting timeouts into "no data received"
@@ -110,7 +113,8 @@ pub fn main<T: Sync + Send>(
         serial_config,
     )?;
 
-    let mut device = Device::new(serial);
+    let (tx, rx) = serial.split();
+    let mut device = GpsModule::new(tx, rx);
 
     // Configure the device to talk UBX
     device
@@ -127,25 +131,30 @@ pub fn main<T: Sync + Send>(
                 reserved5: 0,
             }
             .into_packet_bytes(),
-        )
-        .unwrap();
+        )?;
     device.wait_for_ack::<CfgPrtUart>(Duration::from_millis(3000)).unwrap();
     println!("CfgPrtUart acked!");
 
-    // Enable the NavPosVelTime packet
-    device
-        .write_all(
-            &CfgMsgAllPortsBuilder::set_rate_for::<NavPosVelTime>([0, 0, 1, 0, 0, 0])
+    // Set interval for the NavPosVelTime packet
+    println!("Sending set_rate_for::<NavPosVelTime> ...");
+    for i in 1..5 {
+        device
+            .write_all(
+                &CfgMsgAllPortsBuilder::set_rate_for::<NavPosVelTime>([0, 1, 1, 0, 0, 0])
                 .into_packet_bytes(),
-        )
-        .unwrap();
-    device.wait_for_ack::<CfgMsgAllPorts>(Duration::from_millis(3000)).unwrap();
-    println!("CfgMsgAllPorts acked!");
+            )
+            .unwrap();
+        println!("SENT set_rate_for::<NavPosVelTime>({}) !!!", i);
+        if let Ok(true) = device.wait_for_ack::<CfgMsgSinglePort>(Duration::from_millis(3000)) {
+            println!("Setting rate for NavPosVelTime acked! Exiting loop ...");
+            break
+        }
+    }
 
     // Send a packet request for the MonVer packet
-    device
-        .write_all(&UbxPacketRequest::request_for::<MonVer>().into_packet_bytes())
-        .unwrap();
+    //device
+    //    .write_all(&UbxPacketRequest::request_for::<MonVer>().into_packet_bytes())
+    //    .unwrap();
 
     // Start reading data
     println!("Opened u-blox device, waiting for solutions...");
@@ -181,7 +190,8 @@ pub fn main<T: Sync + Send>(
                 _ => {
                     println!("{:?}", packet);
                 }
-            })?
+            })?;
+        thread::sleep(Duration::from_millis(1000));
     }
 
     println!("exiting GPS sender loop :)");