diff --git a/.gitignore b/.gitignore index 73a638b..bfa5184 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ /.embuild /target /Cargo.lock +/secret diff --git a/Cargo.toml b/Cargo.toml index 4ff3999..5eaa44d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,7 +19,9 @@ esp-idf-hal = "0.37.4" esp-idf-sys = { version = "0.31.5", features = ["binstart", "native"] } mqtt-protocol = "0.11.2" nb = "1.0.0" -ublox = "0.4.2" +nmea0183 = "0.3.0" +serde-json-core = "0.5.0" +serde = "*" [build-dependencies] embuild = "0.29" diff --git a/rust-toolchain.toml b/rust-toolchain.toml index a2f5ab5..bc9d06e 100644 --- a/rust-toolchain.toml +++ b/rust-toolchain.toml @@ -1,2 +1,2 @@ [toolchain] -channel = "esp" +channel = "esp-1.69.0.0" diff --git a/sdkconfig.defaults b/sdkconfig.defaults index cd6dab8..6e54cee 100644 --- a/sdkconfig.defaults +++ b/sdkconfig.defaults @@ -1,5 +1,5 @@ # Rust often needs a bit of an extra main task stack size compared to C (the default is 3K) -CONFIG_ESP_MAIN_TASK_STACK_SIZE=64000 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=128000 # Use this to set FreeRTOS kernel tick frequency to 1000 Hz (100 Hz by default). # This allows to use 1 ms granuality for thread sleeps (10 ms by default). diff --git a/secret/.keep b/secret/.keep new file mode 100644 index 0000000..e69de29 diff --git a/src/command.rs b/src/command.rs index 00fb9ed..13c5370 100644 --- a/src/command.rs +++ b/src/command.rs @@ -12,7 +12,7 @@ impl Command { Command { text: "ATI".to_string(), timeout: Duration::from_millis(6000), - contains: Some("+CIEV".to_string()), + contains: Some("OK".to_string()), } } @@ -236,7 +236,7 @@ impl Command { Command { text: "AT".to_string(), timeout: Duration::from_millis(3000), - contains: Some("+CIEV".to_string()), + contains: Some("OK".to_string()), } } diff --git a/src/config.rs b/src/config.rs index 62f368f..67e62e5 100644 --- a/src/config.rs +++ b/src/config.rs @@ -1,11 +1,19 @@ +#![allow(dead_code)] + pub struct GprsAp<'a> { pub apn: &'a str, pub username: &'a str, pub password: &'a str, } -pub const A1_GPRS_AP: GprsAp = GprsAp { +pub const A1: GprsAp = GprsAp { apn: "internet", username: "internet", password: "internet", }; + +pub const MTS: GprsAp = GprsAp { + apn: "gprswap", + username: "mts", + password: "064", +}; diff --git a/src/gps.rs b/src/gps.rs index 2b0b943..40e84af 100644 --- a/src/gps.rs +++ b/src/gps.rs @@ -3,96 +3,41 @@ use anyhow; use std::{ sync::mpsc::SyncSender, thread, - time::{Duration, Instant}, - io::{Read, Write}, + time::Duration, + io::Read, }; use esp_idf_hal::prelude::*; use esp_idf_hal::serial::{self, Rx, Tx}; -use ublox::*; +use nmea0183::{Parser, ParseResult, Sentence, Source}; -use crate::types::*; +use crate::types::Msg; use crate::serial::SerialIO; struct GpsModule { port: SerialIO, - parser: Parser>, } impl GpsModule { pub fn new(tx: Tx, rx: Rx) -> Self { - let parser = Parser::default(); - GpsModule { port: SerialIO::new(tx, rx), parser } - } - - pub fn write_all(&mut self, data: &[u8]) -> std::io::Result<()> { - println!("WRITE: {:?}", data); - self.port.write(data).map(|_| ()) - } - - pub fn update(&mut self, mut cb: T) -> std::io::Result<()> { - println!("UPDATING ... ... ..."); - loop { - 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 - // returns an iterator-like object we can use to process the packets - let mut it = self.parser.consume(&local_buf[..nbytes]); - while let Some(Ok(packet)) = it.next() { - println!("READ: {:?}", packet); - cb(packet); - } - } - Ok(()) - } - - pub fn wait_for_ack(&mut self, timeout: Duration) -> std::io::Result { - let mut found_packet = false; - println!("LOOKING FOR ACK ..."); - let start = Instant::now(); - while !found_packet && start.elapsed() < timeout { - self.update(|packet| { - if let PacketRef::AckAck(ack) = packet { - if ack.class() == T::CLASS && ack.msg_id() == T::ID { - println!("FOUND PACKET: {} {}", ack.class(), ack.msg_id()); - found_packet = true; - } - } - else if let PacketRef::AckNak(nak) = packet { - println!("NAK PACKET: {} {}", nak.class(), nak.msg_id()); - } - })?; - } - println!("exiting wait_for_ack"); - Ok(found_packet) - } - - /// Reads the serial port, converting timeouts into "no data received" - fn read_port(&mut self, output: &mut [u8]) -> std::io::Result { - match self.port.read(output) { - Ok(b) => Ok(b), - Err(e) => { - if e.kind() == std::io::ErrorKind::TimedOut { - Ok(0) - } else { - Err(e) - } - } + GpsModule { + port: SerialIO::new(tx, rx), } } } -pub fn main( - tx: esp_idf_hal::gpio::Gpio12, - rx: esp_idf_hal::gpio::Gpio13, +pub fn main +( + tx: PTx, + rx: PRx, uart: serial::UART2, sender: SyncSender, -) -> Result<(), anyhow::Error> { +) -> std::result::Result<(), anyhow::Error> +where + PRx: esp_idf_hal::gpio::Pin + esp_idf_hal::gpio::InputPin + esp_idf_hal::gpio::OutputPin, + PTx: esp_idf_hal::gpio::Pin + esp_idf_hal::gpio::InputPin + esp_idf_hal::gpio::OutputPin, +{ let serial_pins = serial::Pins { tx, rx, @@ -100,100 +45,46 @@ pub fn main( rts: None, }; - let serial_config = serial::config::Config::default() - .baudrate(Hertz(9600)) - .data_bits(serial::config::DataBits::DataBits8) - .parity_none() - .flow_control(serial::config::FlowControl::None) - .stop_bits(serial::config::StopBits::STOP1); - let serial: serial::Serial = serial::Serial::new( uart, serial_pins, - serial_config, + serial::config::Config::default().baudrate(Hertz(9600)), )?; let (tx, rx) = serial.split(); let mut device = GpsModule::new(tx, rx); - // Configure the device to talk UBX - device - .write_all( - &CfgPrtUartBuilder { - portid: UartPortId::Uart1, - reserved0: 0, - tx_ready: 0, - mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), - baud_rate: 9600, - in_proto_mask: InProtoMask::all(), - out_proto_mask: OutProtoMask::UBOX, - flags: 0, - reserved5: 0, - } - .into_packet_bytes(), - )?; - device.wait_for_ack::(Duration::from_millis(3000)).unwrap(); - println!("CfgPrtUart acked!"); + let mut parser = Parser::new() + .source_only(Source::GPS) + .sentence_filter(Sentence::GLL | Sentence::GGA); - // Set interval for the NavPosVelTime packet - println!("Sending set_rate_for:: ..."); - for i in 1..5 { - device - .write_all( - &CfgMsgAllPortsBuilder::set_rate_for::([0, 1, 1, 0, 0, 0]) - .into_packet_bytes(), - ) - .unwrap(); - println!("SENT set_rate_for::({}) !!!", i); - if let Ok(true) = device.wait_for_ack::(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::().into_packet_bytes()) - // .unwrap(); - - // Start reading data - println!("Opened u-blox device, waiting for solutions..."); - for _ in 0..20 { - device - .update(|packet| match packet { - PacketRef::MonVer(packet) => { - println!( - "SW version: {} HW version: {}", - packet.software_version(), - packet.hardware_version() - ); - println!("{:?}", packet); - } - PacketRef::NavPosVelTime(sol) => { - let has_posvel = sol.fix_type() == GpsFix::Fix3D - || sol.fix_type() == GpsFix::GPSPlusDeadReckoning; - - if has_posvel { - let pos: Position = (&sol).into(); - let vel: Velocity = (&sol).into(); - let solution = Solution { - latitude: pos.lat, - longitude: pos.lon, - altitude: pos.alt, - speed: vel.speed, - direction: vel.heading, - }; - println!("Sol: {:?}", solution); - sender.send(Msg::Gps(solution)); + let mut c = 0; + let mut nmea = [0_u8; 1024]; + loop { + if let Ok(_) = device.port.read(nmea.as_mut_slice()) { + println!("\r\n\r\n\r\n\r\n"); + for result in parser.parse_from_bytes(&nmea[..]) { + match result { + Ok(ParseResult::GLL(Some(gll))) => { + sender.send(Msg::Gps(gll.into()))?; + }, + Ok(ParseResult::GGA(Some(gga))) => { + sender.send(Msg::Gps(gga.into()))?; } + _ => { } } - _ => { - println!("{:?}", packet); - } - })?; - thread::sleep(Duration::from_millis(1000)); + } + c = 0; + } else { + println!("nothing to read after {} tries ...", c); + if c > 100 { + println!("reached {} retries ... bailing!", c); + break; + } + } + thread::sleep(Duration::from_millis(5000)); + c += 1; } - println!("exiting GPS sender loop :)"); Ok(()) } diff --git a/src/main.rs b/src/main.rs index e4449fd..e6b4325 100644 --- a/src/main.rs +++ b/src/main.rs @@ -5,10 +5,15 @@ mod command; mod modem; mod serial; mod types; +mod gps; use anyhow; use std::{thread::{self, JoinHandle}, time::Duration}; use esp_idf_hal::peripherals::Peripherals; +use esp_idf_hal::prelude::*; +use esp_idf_hal::serial::{Pins, config::Config, Serial, UART1, Uart}; +use embedded_hal::digital::v2::OutputPin; + use types::*; fn main() -> anyhow::Result<()> { @@ -18,6 +23,25 @@ fn main() -> anyhow::Result<()> { println!("Rust main thread: {:?}", thread::current()); + let mut threads: Vec>> = vec![]; + + // // Rx/Tx pins for the GPS modem + let gps_rx = dp.pins.gpio32; + let gps_tx = dp.pins.gpio33; + // // UART interface for the GPS modem + let gps_uart = dp.uart2; + + + let (gps_sender, receiver) = std::sync::mpsc::sync_channel::(3); + //let accel_sender = gps_sender.clone(); + + //let _ = gps::main(gps_tx, gps_rx, gps_uart, gps_sender)?; + //threads.push(thread::spawn(move || accel::main(accel_sender))); + + // ================================== + // MODEM INITIALIZATION AND MAIN LOOP + // ================================== + // LilyGo TTGO T-Call sim800l board serial pins. let modem_rx = dp.pins.gpio26; let modem_tx = dp.pins.gpio27; @@ -28,24 +52,38 @@ fn main() -> anyhow::Result<()> { // UART interface for the GSM modem let modem_uart = dp.uart1; - let mut threads: Vec>> = vec![]; + let serial_pins = Pins { + tx: modem_tx, + rx: modem_rx, + cts: None, + rts: None, + }; - // // Rx/Tx pins for the GPS modem - // let gps_rx = dp.pins.gpio13; - // let gps_tx = dp.pins.gpio12; - // // UART interface for the GPS modem - // let gps_uart = dp.uart2; + let serial: Serial = Serial::new( + modem_uart, + serial_pins, + Config::default().baudrate(Hertz(115200)), + )?; + let (tx, rx) = serial.split(); + type PwrkeyOutput = esp_idf_hal::gpio::Gpio4; + type ResetOutput = esp_idf_hal::gpio::Gpio5; + type PowerOutput = esp_idf_hal::gpio::Gpio23; - let (gps_sender, receiver) = std::sync::mpsc::sync_channel::(1); - let accel_sender = gps_sender.clone(); + let mut mdm: modem::Modem = modem::Modem::new(tx, rx, modem_pwrkey, modem_rst, modem_power, receiver); - // let _ = gps::main(gps_tx, gps_rx, gps_uart, gps_sender)?; - // threads.push(thread::spawn(move || gps::main(gps_rx, gps_tx, gps_uart, gps_sender))); - thread::sleep(Duration::from_millis(1000)); - threads.push(thread::spawn(move || accel::main(accel_sender))); + let mqtt_username = include_str!("../secret/username").trim(); + let mqtt_password = include_str!("../secret/password").trim(); - let _ = modem::main(modem_rx, modem_tx, modem_uart, modem_pwrkey, modem_rst, modem_power, receiver)?; + threads.push(thread::spawn(move || gps::main(gps_tx, gps_rx, gps_uart, gps_sender.clone()))); - Ok(()) + println!("======================= MAIN ======================="); + mdm.init().unwrap_or(()); + let _ = mdm.echo(false).unwrap_or(()); + println!("resetting modem ... "); + println!("======================= MODEM ======================="); + let _ = mdm.mqtt_send_position_loop("51.158.66.64", 7887, mqtt_username, mqtt_password).unwrap_or(()); + let _ = mdm.tcp_close_connection().unwrap_or(()); + thread::sleep(Duration::from_millis(1500)); + panic!("rebooting"); } diff --git a/src/modem.rs b/src/modem.rs index cc313c6..953fc1d 100644 --- a/src/modem.rs +++ b/src/modem.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] + use crate::command::Command; use crate::serial::SerialIO; use crate::types::*; @@ -11,20 +13,25 @@ use std::{ sync::mpsc::Receiver, }; -use esp_idf_hal::prelude::*; use esp_idf_hal::serial::{self, Rx, Tx}; use embedded_hal::digital::v2::OutputPin; -use mqtt::packet::{ConnectPacket, PublishPacket, QoSWithPacketIdentifier, VariablePacket}; -use mqtt::{Encodable, Decodable, TopicName}; +use mqtt::{ + Encodable, + Decodable, + TopicName, + packet::{ + ConnectPacket, + PublishPacket, + QoSWithPacketIdentifier, + VariablePacket, + }, +}; +use serde_json_core; pub type Result = std::result::Result; -pub struct Modem { - serial: SerialIO, -} - #[derive(Debug)] pub enum ModemError { CommandError(String), @@ -42,10 +49,22 @@ impl std::fmt::Display for ModemError { } } -impl Modem { - pub fn new(tx: Tx, rx: Rx) -> Self { +pub struct Modem { + serial: SerialIO, + reset: RST, + power: PW, + power_key: PWK, + receiver: Receiver, +} + +impl Modem { + pub fn new(tx: Tx, rx: Rx, mut pwrkey: PWK, mut rst: RST, mut power: PW, receiver: Receiver) -> Self { Self { serial: SerialIO::new(tx, rx), + reset: rst, + power, + power_key: pwrkey, + receiver, } } @@ -64,30 +83,31 @@ impl Modem { /// /// modem::init(modem_pwrkey, modem_rst, modem_power); /// ``` - pub fn init(&mut self, mut pwrkey: impl OutputPin, mut rst: impl OutputPin, mut power: impl OutputPin) -> Result<()> { + pub fn init(&mut self) -> Result<()> { println!("Turning SIM800L on ..."); - power.set_high().map_err(|_| ModemError::SetupError("Error setting POWER to high.".to_string()))?; - //rst.set_high().map_err(|_| ModemError::SetupError("Error setting RST to high.".to_string()))?; + self.power.set_high().map_err(|_| ModemError::SetupError("Error setting POWER to high.".to_string()))?; + self.reset.set_high().map_err(|_| ModemError::SetupError("Error setting RST to high.".to_string()))?; // Pull down PWRKEY for more than 1 second according to manual requirements - pwrkey.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?; + self.power_key.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?; thread::sleep(Duration::from_millis(1500)); - pwrkey.set_low().map_err(|_| ModemError::SetupError("Error setting PWRKEY to low.".to_string()))?; + self.power_key.set_low().map_err(|_| ModemError::SetupError("Error setting PWRKEY to low.".to_string()))?; thread::sleep(Duration::from_millis(1000)); - pwrkey.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?; + self.power_key.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?; println!("Waiting for sim module to come online ..."); thread::sleep(Duration::from_millis(3000)); - loop { - match self.send_command(Command::probe()) { - Ok(_) => break, - _ => { - thread::sleep(Duration::from_millis(2000)); - continue - }, - } + for _ in 0..10 { + let _ = self.send_command(Command::probe()).unwrap_or("".to_string()); + thread::sleep(Duration::from_millis(1000)); } + self.serial.clear(); Ok(()) } + pub fn echo(&mut self, enabled: bool) -> Result<()> { + let cmd = format!("ATE{}", if enabled { 1 } else { 0 }); + self.send(&cmd, "OK").map(|_| ()) + } + /// Reads the serial RX until the `contains` string is encoutered if `contains` is Some(s), if /// None, then the first line is returned. If a timeout is reached. The timeout is provided on /// input via the `timeout` argument. The first argument `contains` is checked against every @@ -114,7 +134,7 @@ impl Modem { if response.contains(&c) { Ok(response) } else { - Err(ModemError::CommandError(format!("Didn't get expected ({}) from modem.", c))) + Err(ModemError::CommandError(format!("Didn't get expected ({}) from modem. Got: {}", c, response))) } } else { Ok(response) @@ -130,6 +150,7 @@ impl Modem { } fn send(&mut self, at_command: &str, contains: &str) -> Result { + self.serial.clear(); println!("-----------------------------------------------------------"); println!("Sending {} ...", at_command); @@ -171,7 +192,7 @@ impl Modem { let _ = self.handle_prompt()?; println!("Handled prompt OK!!"); - println!("Writing bytes in serial TX! ({:?})", String::from_utf8(buf.to_vec())); + println!("Writing bytes in serial TX! ({:?})", buf.into_iter().map(|b| char::from(*b)).collect::()); self.serial .write_bytes(buf) .map_err(|err| ModemError::SendDataError(format!("{:?}", err)))?; @@ -197,15 +218,15 @@ impl Modem { self.send_command(Command::gprs_bearer_status()) } - pub fn gprs_attach_ap(&mut self, apn: &str, username: &str, password: &str)-> Result<()> { + pub fn gprs_attach_ap(&mut self, config: crate::config::GprsAp)-> Result<()> { println!("init gprs ..."); let _ = self.send_command(Command::gprs_init())?; - println!("setting up gprs credentials for apn {}, {}:{})", apn, username, password); + println!("setting up gprs credentials for apn {}, {}:{})", config.apn, config.username, config.password); - let _ = self.send_command(Command::gprs_set_apn(apn))?; - let _ = self.send_command(Command::gprs_set_user(username))?; - let _ = self.send_command(Command::gprs_set_pwd(password))?; + let _ = self.send_command(Command::gprs_set_apn(config.apn))?; + let _ = self.send_command(Command::gprs_set_user(config.username))?; + let _ = self.send_command(Command::gprs_set_pwd(config.password))?; Ok(()) } @@ -223,6 +244,7 @@ impl Modem { fn try_connect_gprs(&mut self) -> Result<()> { let mut retries = 0; + println!("TRYING TO CONNECT TO GPRS"); loop { if self.is_gprs_attached()? { let _ = self.gprs_connect()?; @@ -258,17 +280,29 @@ impl Modem { pub fn tcp_connect(&mut self, addr: &str, port: u16) -> Result<()> { let at_command = format!("AT+CIPSTART=\"TCP\",\"{}\",\"{}\"", addr, port); - let _ = self.send(&at_command, "CONNECT OK"); + let mut reply_result = self.send(&at_command, "CONNECT OK"); for _ in 0..3 { - if let Ok(reply) = self.command_read_response(Some("CONNECT OK".to_string())) { + if let Ok(reply) = reply_result { println!("TCP connect replied with {}", reply); break + } else { + reply_result = self.command_read_response(Some("CONNECT OK".to_string())); } - thread::sleep(Duration::from_millis(500)); + thread::sleep(Duration::from_millis(1000)); } Ok(()) } + pub fn tls_connect(&mut self, addr: &str, port: u16) -> Result<()> { + let _ = self.tcp_connect(addr, port)?; + + // ------------------------ + // TLS handshake goes here. + // ------------------------ + + Ok(()) + } + pub fn tcp_set_quick_mode(&mut self, mode: bool) -> Result<()> { self.send_command(Command::tcp_set_quick_mode(mode)) .map(|_| ()) @@ -280,6 +314,8 @@ impl Modem { } pub fn tcp_manual_send(&mut self, buf: &[u8]) -> Result<()> { + thread::sleep(Duration::from_millis(200)); + // self.serial.clear(); self.tcp_manual_send_data(buf) .map(|_| ()) } @@ -335,8 +371,8 @@ impl Modem { } } - pub fn tcp_close_connection(&mut self) -> Result { - self.send_command(Command::tcp_close()) + pub fn tcp_close_connection(&mut self) -> Result<()> { + self.send_command(Command::tcp_close()).map(|_| ()) } pub fn http_post(&mut self, url: &str, token: &str, content: &[u8]) -> Result { @@ -450,13 +486,15 @@ impl Modem { Err(ModemError::ReadError("TCP server didn't respond!".into())) } - fn mqtt_connect(&mut self, device_id: &str) -> anyhow::Result<()> { + fn mqtt_connect(&mut self, device_id: &str, username: &str, password: &str) -> anyhow::Result<()> { let mut buf = Vec::new(); let mut conn = ConnectPacket::new(device_id); conn.set_clean_session(true); conn.set_keep_alive(100); + conn.set_user_name(Some(username.to_string())); + conn.set_password(Some(password.to_string())); let _ = conn.encode(&mut buf)?; - self.tcp_manual_send(&mut buf).ok(); + let _ = self.tcp_manual_send(&mut buf)?; let reply = self.mqtt_receive_reply()?; println!("mqtt decoded packet: ({:?})", reply); @@ -476,114 +514,66 @@ impl Modem { message.as_bytes(), ); let _ = packet.encode(&mut buf)?; - println!("created mqtt publish packet ... ({})", - std::str::from_utf8(buf.as_slice()).unwrap_or("")); - self.tcp_manual_send(&mut buf).ok(); + self.tcp_manual_send(&mut buf)?; + Ok(()) + } + + pub fn mqtt_send_position_loop(&mut self, host: &str, port: u16, username: &str, password: &str) -> anyhow::Result<()> { + if !self.is_gprs_attached()? { + let _ = self.gprs_attach_ap(crate::config::MTS)?; + let _ = self.try_connect_gprs()?; + } + // When command AT+CIPQSEND=0, it is in normal sending mode. In this mode, after user + // sends data by AT+CIPSEND, if the server receives TCP data, it will give ACK message + // to module, and the module will respond SEND OK. + let _ = self.send("AT+CIPQSEND=0", "OK"); + // Enables getting data from network manually. + let _ = self.send("AT+CIPRXGET=1", "OK"); + + for _ in 0..5 { + if let Ok(_) = self.tcp_connect(host, port) { + break + } + } + + let device_id = "c36a72df-5bd6-4f9b-995d-059433bc3267"; + println!("connecting to MQTT with ({}:{})", username, password); + let _ = self.mqtt_connect(device_id, username, password)?; + + println!("entering queue receive loop ..."); + let mut err_count = 0; + let _ = loop { + match self.receiver.recv() { + Ok(Msg::Gps(solution)) => { + println!("received GPS solution {:?} | sending to mqtt ...", solution); + serde_json_core::ser::to_string::(&solution) + .map_err(|e| anyhow::Error::new(e)) + .and_then(|sol| self.mqtt_publish(device_id, &sol))?; + err_count = 0; + }, + Ok(Msg::Accelerometer(acc)) => { + println!("received accel {} | sending to mqtt ...", acc); + let _ = self.mqtt_publish(device_id, &format!("{:?}", acc))?; + err_count = 0; + } + Err(e) => { + if err_count < 5 { + err_count += 1; + println!("received error {} | NOT sending to mqtt ...", e); + } + else { + break + } + } + } + }; + Ok(()) } } -impl std::io::Read for Modem { +impl std::io::Read for Modem { fn read(&mut self, buf: &mut [u8]) -> std::io::Result { self.tcp_receive(buf).map_err(|_| std::io::Error::from(std::io::ErrorKind::ConnectionAborted)) } } - -pub fn main( - rx: esp_idf_hal::gpio::Gpio26, - tx: esp_idf_hal::gpio::Gpio27, - uart: serial::UART1, - pwrkey: esp_idf_hal::gpio::Gpio4, - rst: esp_idf_hal::gpio::Gpio5, - power: esp_idf_hal::gpio::Gpio23, - receiver: Receiver, -) -> std::result::Result<(), anyhow::Error> { - let serial_pins = serial::Pins { - tx, - rx, - cts: None, - rts: None, - }; - - let serial: serial::Serial = serial::Serial::new( - uart, - serial_pins, - serial::config::Config::default().baudrate(Hertz(115200)), - )?; - - let (tx, rx) = serial.split(); - let mut mdm = Modem::new(tx, rx); - - mdm.init(pwrkey, rst, power)?; - - // thread::sleep(Duration::from_millis(500)); - - //println!("setting up client TLS cert"); - //let client_cert = include_bytes!("../certs/full-bin.p12"); - //let client_cert_path = "C:\\USER\\fullchain.pem"; - - //let _ = mdm.upload_cert(client_cert_path, client_cert)?; - //let _ = mdm.ssl_set_client_cert(client_cert_path, "t")?; - //let _ = mdm.fs_list("C:\\USER\\")?; - - // Retry 5 times to open a TCP connection, otherwise fail and wait for reboot. - let mut retries = 0; - - loop { - if !mdm.is_gprs_attached()? { - let _ = mdm.gprs_attach_ap( - crate::config::A1_GPRS_AP.apn, - crate::config::A1_GPRS_AP.username, - crate::config::A1_GPRS_AP.password, - )?; - } - if let Ok(()) = mdm.try_connect_gprs() { - let device_id = "c36a72df-5bd6-4f9b-995d-059433bc3267"; - - // When command AT+CIPQSEND=0, it is in normal sending mode. In this mode, after user - // sends data by AT+CIPSEND, if the server receives TCP data, it will give ACK message - // to module, and the module will respond SEND OK. - let _ = mdm.send("AT+CIPQSEND=0", "OK"); - // Enables getting data from network manually. - let _ = mdm.send("AT+CIPRXGET=1", "OK"); - - if let Err(_) = mdm.tcp_connect("51.158.66.64", 7887) { - if retries < 5 { - retries += 1; - continue; - } - } - thread::sleep(Duration::from_millis(500)); - mdm.serial.clear(); - - let _ = mdm.mqtt_connect(device_id)?; - - println!("entering queue receive loop ..."); - let mut err_count = 0; - let _ = loop { - match receiver.recv() { - Ok(Msg::Gps(solution)) => { - println!("received GPS solution {:?} | sending to mqtt ...", solution); - let _ = mdm.mqtt_publish(device_id, &format!("{:?}", solution))?; - err_count = 0; - }, - Ok(Msg::Accelerometer(acc)) => { - println!("received accel {} | sending to mqtt ...", acc); - let _ = mdm.mqtt_publish(device_id, &format!("{:?}", acc))?; - err_count = 0; - } - Err(e) => { - if err_count < 5 { - err_count += 1; - println!("received error {} | NOT sending to mqtt ...", e); - } - else { - break - } - } - } - }; - let _ = mdm.tcp_close_connection()?; - } - } -} diff --git a/src/types.rs b/src/types.rs index daed150..89c186d 100644 --- a/src/types.rs +++ b/src/types.rs @@ -1,14 +1,106 @@ -#[derive(Debug)] +#![allow(dead_code)] +use nmea0183::{ + GGA, + GLL, + coords::{ + Latitude as NMEALatitude, + Longitude as NMEALongitude, + Hemisphere as NMEAHemisphere, + }, +}; +use serde::Serialize; + +#[derive(Debug, Serialize)] +pub enum Hemisphere { + North, + South, + East, + West, +} + +#[derive(Debug, Serialize)] +pub struct Latitude { + degrees: u8, + minutes: u8, + seconds: f32, + hemisphere: Hemisphere, +} + +#[derive(Debug, Serialize)] +pub struct Longitude { + degrees: u8, + minutes: u8, + seconds: f32, + hemisphere: Hemisphere, +} + +impl From for Latitude { + fn from(lat: NMEALatitude) -> Self { + Self { + degrees: lat.degrees, + minutes: lat.minutes, + seconds: lat.seconds, + hemisphere: lat.hemisphere.into(), + } + } +} + +impl From for Longitude { + fn from(lon: NMEALongitude) -> Self { + Self { + degrees: lon.degrees, + minutes: lon.minutes, + seconds: lon.seconds, + hemisphere: lon.hemisphere.into(), + } + } +} + +impl From for Hemisphere { + fn from(hem: NMEAHemisphere) -> Self { + match hem { + NMEAHemisphere::North => Self::North, + NMEAHemisphere::South => Self::South, + NMEAHemisphere::East => Self::East, + NMEAHemisphere::West => Self::West, + } + } +} + +#[derive(Debug, Serialize)] pub struct Solution { - pub latitude: f64, - pub longitude: f64, - pub altitude: f64, - pub speed: f64, - pub direction: f64, + pub latitude: Latitude, + pub longitude: Longitude, + pub altitude: Option, + pub speed: Option, + pub direction: Option, +} + +impl From for Solution { + fn from(gga: GGA) -> Self { + Self { + latitude: gga.latitude.into(), + longitude: gga.longitude.into(), + altitude: Some(gga.altitude.meters), + speed: None, + direction: None, + } + } +} + +impl From for Solution { + fn from(gll: GLL) -> Self { + Self { + latitude: gll.latitude.into(), + longitude: gll.longitude.into(), + altitude: None, + speed: None, + direction: None, + } + } } pub enum Msg { Gps(Solution), Accelerometer(String), } -