diff --git a/pico/Cargo.lock b/pico/Cargo.lock index f1b3568..c3f03c3 100644 --- a/pico/Cargo.lock +++ b/pico/Cargo.lock @@ -449,7 +449,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-sync 0.7.2", - "embassy-time 0.5.0", + "embassy-time 0.5.1", "embassy-time-driver", "embassy-time-queue-utils", "embassy-usb-driver", @@ -519,9 +519,9 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.5.0" +version = "0.5.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f4fa65b9284d974dad7a23bb72835c4ec85c0b540d86af7fc4098c88cff51d65" +checksum = "592b0c143ec626e821d4d90da51a2bd91d559d6c442b7c74a47d368c9e23d97a" dependencies = [ "cfg-if", "critical-section", @@ -536,9 +536,9 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.1" +version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a0a244c7dc22c8d0289379c8d8830cae06bb93d8f990194d0de5efb3b5ae7ba6" +checksum = "6ee71af1b3a0deaa53eaf2d39252f83504c853646e472400b763060389b9fcc9" dependencies = [ "document-features", ] @@ -1585,8 +1585,9 @@ dependencies = [ "embassy-futures", "embassy-rp", "embassy-sync 0.6.2", - "embassy-time 0.5.0", + "embassy-time 0.5.1", "embedded-alloc", + "fasttime", "panic-probe", "pico-lib", "portable-atomic", diff --git a/pico/app/Cargo.toml b/pico/app/Cargo.toml index 132c5d9..994d5cc 100644 --- a/pico/app/Cargo.toml +++ b/pico/app/Cargo.toml @@ -39,6 +39,8 @@ atat = { version = "0.24.1", features = ["defmt", "heapless"] } static_cell = { version = "2" } portable-atomic = { version = "1.13.1", features = ["critical-section"] } +fasttime = { version = "0.1", default-features = false } + [[bin]] name = "tATA-pico" path = "src/main.rs" diff --git a/pico/app/src/main.rs b/pico/app/src/main.rs index 42fa34e..4b098cb 100644 --- a/pico/app/src/main.rs +++ b/pico/app/src/main.rs @@ -1,32 +1,30 @@ #![no_std] #![no_main] -use alloc::string::ToString; use atat::asynch::Client; use atat::heapless::String; use atat::{AtatIngress, DefaultDigester, Ingress, ResponseSlot, UrcChannel}; use core::ptr::addr_of_mut; use defmt::*; use embassy_executor::Spawner; -use embassy_futures::select::{Either3, select3}; +use embassy_futures::select::{Either, select}; use embassy_rp::adc::{Adc, Channel, Config, InterruptHandler as AdcInterruptHandler}; use embassy_rp::bind_interrupts; use embassy_rp::gpio::{Level, Output, Pull}; -use embassy_rp::peripherals::UART0; +use embassy_rp::peripherals::{RTC, UART0}; use embassy_rp::rtc::{DateTime, DateTimeFilter, DayOfWeek, Rtc}; use embassy_rp::uart::{self, BufferedInterruptHandler, BufferedUart, BufferedUartRx}; use embassy_sync::pubsub; use embassy_time::{Duration, Timer}; use embedded_alloc::LlffHeap as Heap; +use pico_lib::service::{Configuration, DeviceStatus, Service}; use static_cell::StaticCell; use {defmt_rtt as _, panic_probe as _}; use embassy_rp::watchdog::Watchdog; +use pico_lib::at; use pico_lib::at::PicoHW; -use pico_lib::poro; use pico_lib::urc; -use pico_lib::utils::{astring_to_string, send_command_logged}; -use pico_lib::{at, battery, call, gps, gsm, network, sms}; extern crate alloc; @@ -53,9 +51,20 @@ async fn main(spawner: Spawner) { unsafe { HEAP.init(addr_of_mut!(HEAP_MEM) as usize, HEAP_SIZE) } } let p = embassy_rp::init(Default::default()); - let mut rtc = Rtc::new(p.RTC, Irqs); - if !rtc.is_running() { + Timer::after(Duration::from_secs(2)).await; + info!("STARTED"); + + let watchdog = Watchdog::new(p.WATCHDOG); + spawner.spawn(watchdog_task(watchdog)).unwrap(); + + let mut pico = Pico { + led: Output::new(p.PIN_25, Level::Low), + power: Output::new(p.PIN_14, Level::Low), + rtc: Rtc::new(p.RTC, Irqs), + }; + + if !pico.rtc.is_running() { let now = DateTime { year: 2000, month: 1, @@ -65,45 +74,11 @@ async fn main(spawner: Spawner) { minute: 0, second: 0, }; - rtc.set_datetime(now).unwrap(); + pico.rtc.set_datetime(now).unwrap(); // The rp2040 chip will always add a Feb 29th on every year that is divisible by 4, // but this may be incorrect (e.g. on century years) - rtc.set_leap_year_check(false); + pico.rtc.set_leap_year_check(false); } - Timer::after(Duration::from_secs(2)).await; - info!("STARTED"); - - let watchdog = Watchdog::new(p.WATCHDOG); - spawner.spawn(watchdog_task(watchdog)).unwrap(); - - let mut pico = Pico { - led: Output::new(p.PIN_25, Level::Low), - power: Output::new(p.PIN_14, Level::Low), - }; - - // This is just a Test will be removed later. - let pm = poro::ProtectorMachine {}; - let dumped = pm.dump(&poro::Protector { - car_location: Some(poro::CarLocation { - position: poro::Position { - latitude: 46.7624859f64, - longitude: 18.6304591f64, - }, - accuracy: 250.25f32, - battery: 0.8912f32, - timestamp: 1670077542109i64, - }), - park_location: Some(poro::ParkLocation { - position: poro::Position { - latitude: 47.1258945f64, - longitude: 17.8372091f64, - }, - accuracy: 500.25f32, - }), - status: Some(poro::Status::CarTheftDetected), - service: Some(poro::Service { value: true }), - }); - info!("PORO TEST: {}", dumped.as_str()); let mut adc = Adc::new(p.ADC, Irqs, Config::default()); let mut p26 = Channel::new_pin(p.PIN_26, Pull::None); @@ -141,196 +116,96 @@ async fn main(spawner: Spawner) { atat::Config::default(), ); - Timer::after(Duration::from_millis(500)).await; - info!("Before spawning reader Task"); - spawner.spawn(ingress_task(ingress, reader)).unwrap(); - Timer::after(Duration::from_millis(500)).await; - info!("After spawning reader Task"); - let mut sub = URC_CHANNEL.subscribe().unwrap(); - info!("Network init"); - Timer::after(Duration::from_secs(2)).await; - - network::init_network(&mut client, &mut pico).await; - sms::init(&mut client, &mut pico).await; - call::init(&mut client, &mut pico).await; - - for _ in 0..30 { - pico.set_led_high(); - Timer::after(Duration::from_millis(100)).await; - pico.set_led_low(); - Timer::after(Duration::from_millis(100)).await; - } - - match gps::get_gps_location(&mut client, &mut pico, 5).await { - Some(v) => info!("GPS location: {:?}", v), - None => (), - } - - match gsm::get_gsm_location(&mut client, &mut pico, 5, "online").await { - Some(v) => info!("GSM location: {:?}", v), - None => (), - } - - let phone_number: String<30> = String::try_from("+36301234567").unwrap(); - - call::call_number( - &mut client, - &mut pico, - &phone_number, - Duration::from_secs(10).as_millis(), - ) - .await; - - let mut tata_response: String<160> = String::try_from("$tATA/").unwrap(); - let _ = tata_response.push_str(dumped.as_str()); - - sms::send_sms( - &mut client, - &mut pico, - &phone_number, - &astring_to_string(tata_response.as_str()), - ) - .await; - - sms::receive_sms(&mut client, &mut pico).await; + let mut service = Service { + cfg: Configuration { + phone_number: String::try_from("+36301234567").unwrap(), + sms_password: String::try_from("12345").unwrap(), + service_enabled: true, + locator_poll_count: 10, + check_period_seconds: 15 * 60, + call_after_boot: true, + debug_alerts: true, + battery_alerts: true, + detect_parking: true, + keep_n_sms: 20, + }, + status: DeviceStatus { + last_big_location_change: 0, + location: None, + park_location: None, + battery: 100.0f32, + last_battery_alert: 0, + }, + }; - let mut counter = 0u64; - rtc.schedule_alarm(DateTimeFilter::default().second(30)); + service.init(&mut client, &mut pico).await; + pico.rtc + .schedule_alarm(DateTimeFilter::default().second(30)); loop { - // Wait for 5 seconds or until the alarm is triggered - match select3( - Timer::after_secs(4), - rtc.wait_for_alarm(), - sub.next_message(), - ) - .await - { - // Timer expired - Either3::First(_) => { + match select(pico.rtc.wait_for_alarm(), sub.next_message()).await { + // Alarm triggered + Either::First(_) => { pico.set_led_high(); - Timer::after(Duration::from_millis(500)).await; - let dt = rtc.now().unwrap(); + let dt = pico.rtc.now().unwrap(); info!( - "Now: {}-{:02}-{:02} {}:{:02}:{:02}", - dt.year, dt.month, dt.day, dt.hour, dt.minute, dt.second, + "ALARM TRIGGERED! Now: {}-{:02}-{:02} {}:{:02}:{:02} Pin26 ADC: {} Temperature: {}", + dt.year, + dt.month, + dt.day, + dt.hour, + dt.minute, + dt.second, + adc.read(&mut p26).await.unwrap(), + convert_to_celsius(adc.read(&mut ts).await.unwrap()) ); - counter += 1; - let level = adc.read(&mut p26).await.unwrap(); - let temp = convert_to_celsius(adc.read(&mut ts).await.unwrap()); - info!( - "Tick counter: {} Pin 26 ADC: {} Temp: {}", - counter, level, temp - ); + service.refresh(&mut client, &mut pico).await; + // every 10 minute, todo.. + pico.rtc + .schedule_alarm(DateTimeFilter::default().minute((dt.minute + 10) % 60)); pico.set_led_low(); - Timer::after(Duration::from_millis(500)).await; } - // Alarm triggered - Either3::Second(_) => { - let dt = rtc.now().unwrap(); + // Unsolicited Message + Either::Second(m) => { + pico.set_led_high(); + let dt = pico.rtc.now().unwrap(); info!( - "ALARM TRIGGERED! Now: {}-{:02}-{:02} {}:{:02}:{:02}", - dt.year, dt.month, dt.day, dt.hour, dt.minute, dt.second, + "URC! Now: {}-{:02}-{:02} {}:{:02}:{:02} Pin26 ADC: {} Temperature: {}", + dt.year, + dt.month, + dt.day, + dt.hour, + dt.minute, + dt.second, + adc.read(&mut p26).await.unwrap(), + convert_to_celsius(adc.read(&mut ts).await.unwrap()) ); - rtc.schedule_alarm(DateTimeFilter::default().second(30)); - - match gps::get_gps_location(&mut client, &mut pico, 5).await { - Some(v) => info!("GPS location: {:?}", v), - None => (), - } - - match send_command_logged( - &mut client, - &battery::AtBatteryChargeExecute, - "AtBatteryChargeExecute".to_string(), - ) - .await - { - Ok(v) => info!(" {:?}", v), - Err(_) => (), - } - } - Either3::Third(m) => match &m { - pubsub::WaitResult::Message(u) => match u { - urc::Urc::CallReady => { - info!("URC CallReady"); - } - urc::Urc::SMSReady => { - info!("URC SMSReady"); - } - urc::Urc::SetBearer(_v) => { - info!("URC SetBearer"); - } - urc::Urc::GprsDisconnected(_v) => { - info!("URC GprsDisconnected"); - } - urc::Urc::Ring => { - info!("URC Ring"); - } - urc::Urc::NormalPowerDown => { - info!("URC NormalPowerDown"); - } - urc::Urc::UnderVoltagePowerDown => { - info!("URC UnderVoltagePowerDown"); - } - urc::Urc::UnderVoltageWarning => { - info!("URC UnderVoltageWarning"); - } - urc::Urc::OverVoltagePowerDown => { - info!("URC OverVoltagePowerDown"); - } - urc::Urc::OverVoltageWarning => { - info!("URC OverVoltageWarning"); - } - urc::Urc::ChargeOnlyMode => { - info!("URC ChargeOnlyMode"); - call::call_number( - &mut client, - &mut pico, - &phone_number, - Duration::from_secs(10).as_millis(), - ) - .await; - } - urc::Urc::Ready => { - info!("URC Ready"); - } - urc::Urc::ConnectOK1 => { - info!("URC ConnectOK1"); - } - urc::Urc::ConnectOK => { - info!("URC ConnectOK"); - } - urc::Urc::ClipUrc(v) => { - info!("URC ClipUrc number={}, type={}", v.number.as_str(), v.type_); - if v.number == phone_number { - Timer::after_millis(2000).await; - call::answer_incoming_call(&mut client, &mut pico).await; - } else { - call::hangup_incoming_call(&mut client, &mut pico).await; + match &m { + pubsub::WaitResult::Message(u) => match u { + urc::Urc::ClipUrc(v) => { + service + .handle_incoming_call(&mut client, &mut pico, &v.number) + .await; } + urc::Urc::NewMessageIndicationUrc(v) => { + service + .handle_sms(&mut client, &mut pico, v.index as u32) + .await; + } + _ => (), + }, + pubsub::WaitResult::Lagged(b) => { + info!("Urc Lagged messages: {}", b); } - urc::Urc::NewMessageIndicationUrc(v) => { - info!( - "URC NewMessageIndicationUrc index={} mem={}", - v.index, - v.mem.as_str() - ); - } - urc::Urc::EnterPinReadResponse(v) => { - info!("URC EnterPinReadResponse code={}", v.code); - } - }, - pubsub::WaitResult::Lagged(b) => { - info!("Urc Lagged messages: {}", b); } - }, + pico.set_led_low(); + } } } } @@ -373,6 +248,7 @@ fn convert_to_celsius(raw_temp: u16) -> f32 { struct Pico<'a> { led: Output<'a>, power: Output<'a>, + rtc: Rtc<'a, RTC>, } impl at::PicoHW for Pico<'_> { @@ -446,4 +322,115 @@ impl at::PicoHW for Pico<'_> { // received from the serial port every time when SIM868 is powered on. For details, please refer to the chapter // “AT+IPR” in document [1] } + + fn rtc_now_millis(&mut self) -> i64 { + let dt = self.rtc.now().unwrap(); + let now = fasttime::DateTime { + date: fasttime::Date { + year: dt.year as i32, + month: dt.month, + day: dt.day, + }, + time: fasttime::Time { + hour: dt.hour, + minute: dt.minute, + second: dt.second, + nanosecond: 0, + }, + }; + return (now.unix_timestamp_nanos() / 1_000_000) as i64; + } + + fn set_rtc_time(&mut self, millis: i64) { + self.rtc + .set_datetime(millis_to_datetime(millis as u64).unwrap()) + .unwrap(); + } +} + +// NOTE: This is copied from https://github.com/embassy-rs/embassy/blob/main/embassy-rp/src/datetime/epoch.rs +// TODO: update embassy-rp 0.9 -> 0.10 +const EPOCH_YEAR: u16 = 1970; +const DAYS_IN_MONTH: [u8; 12] = [31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31]; +const MS_PER_SECOND: u64 = 1000; +const MS_PER_MINUTE: u64 = 60 * MS_PER_SECOND; +const MS_PER_HOUR: u64 = 60 * MS_PER_MINUTE; +const MS_PER_DAY: u64 = 24 * MS_PER_HOUR; +const EPOCH_DAY_OF_WEEK: u8 = 4; // Thursday + +const fn is_leap_year(year: u16) -> bool { + (year % 4 == 0) && ((year % 100 != 0) || (year % 400 == 0)) +} + +fn days_in_month(year: u16, month: u8) -> u8 { + if month == 2 && is_leap_year(year) { + 29 + } else { + DAYS_IN_MONTH[(month - 1) as usize] + } +} + +fn day_of_week_from_days(days_since_epoch: u32) -> u8 { + ((days_since_epoch + EPOCH_DAY_OF_WEEK as u32) % 7) as u8 +} + +fn millis_to_datetime(millis: u64) -> Result { + // Use u64 for initial division, then cast to u32 for subsequent calculations + // Max total_days for year 4095 is ~776,000, fits in u32 + let total_days = (millis / MS_PER_DAY) as u32; + // remaining_ms is at most MS_PER_DAY - 1 = 86,399,999, fits in u32 + let remaining_ms = (millis % MS_PER_DAY) as u32; + + let hour = (remaining_ms / MS_PER_HOUR as u32) as u8; + let remaining_ms = remaining_ms % MS_PER_HOUR as u32; + let minute = (remaining_ms / MS_PER_MINUTE as u32) as u8; + let second = ((remaining_ms % MS_PER_MINUTE as u32) / MS_PER_SECOND as u32) as u8; + + let day_of_week = match day_of_week_from_days(total_days) { + 0 => DayOfWeek::Sunday, + 1 => DayOfWeek::Monday, + 2 => DayOfWeek::Tuesday, + 3 => DayOfWeek::Wednesday, + 4 => DayOfWeek::Thursday, + 5 => DayOfWeek::Friday, + 6 => DayOfWeek::Saturday, + _ => defmt::panic!(), + }; + + let mut year = EPOCH_YEAR; + let mut days_remaining = total_days; + + loop { + let days_in_year: u32 = if is_leap_year(year) { 366 } else { 365 }; + if days_remaining < days_in_year { + break; + } + days_remaining -= days_in_year; + year += 1; + + if year > 4095 { + return Err("InvalidTimestamp"); + } + } + + let mut month = 1u8; + while month <= 12 { + let days_in_this_month = days_in_month(year, month) as u32; + if days_remaining < days_in_this_month { + break; + } + days_remaining -= days_in_this_month; + month += 1; + } + let day = (days_remaining + 1) as u8; + + Ok(DateTime { + year, + month, + day, + day_of_week, + hour, + minute, + second, + }) } diff --git a/pico/pico-lib/src/at.rs b/pico/pico-lib/src/at.rs index d57eba0..b5fb2d7 100644 --- a/pico/pico-lib/src/at.rs +++ b/pico/pico-lib/src/at.rs @@ -9,6 +9,8 @@ pub trait PicoHW { fn set_led_high(&mut self); fn set_led_low(&mut self); fn restart_module(&mut self) -> impl core::future::Future + Send; + fn rtc_now_millis(&mut self) -> i64; + fn set_rtc_time(&mut self, millis: i64); } #[cfg(test)] @@ -73,6 +75,7 @@ pub mod tests { pub set_led_high_calls: u32, pub set_led_low_calls: u32, pub restart_module_calls: u32, + pub uptime_millis: i64, } impl PicoHW for PicoMock { @@ -91,5 +94,13 @@ pub mod tests { async fn restart_module(&mut self) { self.restart_module_calls += 1; } + + fn rtc_now_millis(&mut self) -> i64 { + return self.uptime_millis; + } + + fn set_rtc_time(&mut self, millis: i64) { + self.uptime_millis = millis; + } } } diff --git a/pico/pico-lib/src/battery.rs b/pico/pico-lib/src/battery.rs index 0f8d2df..4335bde 100644 --- a/pico/pico-lib/src/battery.rs +++ b/pico/pico-lib/src/battery.rs @@ -1,3 +1,5 @@ +use crate::utils::send_command_logged; +use alloc::string::ToString; use atat::atat_derive::AtatCmd; use atat::atat_derive::AtatEnum; use atat::atat_derive::AtatResp; @@ -27,6 +29,19 @@ pub enum BatteryStatus { ChargingFinished = 2, } +pub async fn get_battery( + client: &mut T, + _pico: &mut U, +) -> Option { + send_command_logged( + client, + &AtBatteryChargeExecute, + "AtBatteryChargeExecute".to_string(), + ) + .await + .ok() +} + #[cfg(test)] mod tests { use crate::cmd_serialization_tests; diff --git a/pico/pico-lib/src/gps.rs b/pico/pico-lib/src/gps.rs index 8de6d26..f8d5b6b 100644 --- a/pico/pico-lib/src/gps.rs +++ b/pico/pico-lib/src/gps.rs @@ -318,29 +318,6 @@ pub async fn get_gps_location continue; } - let datetime = bytes_to_string(&resp.utc_date_time.unwrap()); - let (year, rest) = datetime.as_str().split_at(4); - let (month, rest) = rest.split_at(2); - let (day, rest) = rest.split_at(2); - let (hour, rest) = rest.split_at(2); - let (minute, rest) = rest.split_at(2); - let (second, rest) = rest.split_at(2); - let (_, millis) = rest.split_at(1); - - let datetime = DateTime { - date: Date { - year: year.parse().unwrap_or_default(), - month: month.parse().unwrap_or_default(), - day: day.parse().unwrap_or_default(), - }, - time: fasttime::Time { - hour: hour.parse().unwrap_or_default(), - minute: minute.parse().unwrap_or_default(), - second: second.parse().unwrap_or_default(), - nanosecond: millis.parse::().unwrap_or_default() * 1_000_000u32, - }, - }; - send_command_logged( client, &AtGnssPowerControlWrite { @@ -356,7 +333,7 @@ pub async fn get_gps_location latitude: resp.latitude.unwrap(), longitude: resp.longitude.unwrap(), accuracy: utils::estimate_gps_accuracy(pdop), - unix_timestamp_millis: (datetime.unix_timestamp_nanos() / 1_000_000) as i64, + unix_timestamp_millis: get_unix_timestamp_millis(resp.utc_date_time.unwrap()), }); } Err(_) => (), @@ -376,6 +353,95 @@ pub async fn get_gps_location return None; } +pub async fn get_gps_unix_timestamp_millis( + client: &mut T, + pico: &mut U, + max_retries: u8, +) -> i64 { + send_command_logged( + client, + &AtGnssPowerControlWrite { + mode: PowerMode::TurnOn, + }, + "AtGnssPowerControlWrite ON".to_string(), + ) + .await + .ok(); + + // TODO defer { AtGnssPowerControlWrite::TurnOff }; would be better + + for i in 0..max_retries { + pico.sleep(1000).await; + match send_command_logged( + client, + &AtGnssNavigationInformationExecute, + format!("AtGnssNavigationInformationExecute {}", i), + ) + .await + { + Ok(resp) => { + if resp.utc_date_time.is_none() { + continue; + } + + let now = get_unix_timestamp_millis(resp.utc_date_time.unwrap()); + if now > 1767247200000 { + // 2026.01.01 06:00 + send_command_logged( + client, + &AtGnssPowerControlWrite { + mode: PowerMode::TurnOff, + }, + "AtGnssPowerControlWrite OFF".to_string(), + ) + .await + .ok(); + return now; + } + } + Err(_) => (), + } + } + + send_command_logged( + client, + &AtGnssPowerControlWrite { + mode: PowerMode::TurnOff, + }, + "AtGnssPowerControlWrite OFF".to_string(), + ) + .await + .ok(); + + return -1; +} + +fn get_unix_timestamp_millis(utc_date_time: Bytes<18>) -> i64 { + let datetime = bytes_to_string(&utc_date_time); + let (year, rest) = datetime.as_str().split_at(4); + let (month, rest) = rest.split_at(2); + let (day, rest) = rest.split_at(2); + let (hour, rest) = rest.split_at(2); + let (minute, rest) = rest.split_at(2); + let (second, rest) = rest.split_at(2); + let (_, millis) = rest.split_at(1); + + let datetime = DateTime { + date: Date { + year: year.parse().unwrap_or_default(), + month: month.parse().unwrap_or_default(), + day: day.parse().unwrap_or_default(), + }, + time: fasttime::Time { + hour: hour.parse().unwrap_or_default(), + minute: minute.parse().unwrap_or_default(), + second: second.parse().unwrap_or_default(), + nanosecond: millis.parse::().unwrap_or_default() * 1_000_000u32, + }, + }; + return (datetime.unix_timestamp_nanos() / 1_000_000) as i64; +} + #[cfg(test)] extern crate std; diff --git a/pico/pico-lib/src/lib.rs b/pico/pico-lib/src/lib.rs index 5d1a54c..33810f2 100644 --- a/pico/pico-lib/src/lib.rs +++ b/pico/pico-lib/src/lib.rs @@ -11,6 +11,7 @@ pub mod hexstr; pub mod location; pub mod network; pub mod poro; +pub mod service; pub mod sms; pub mod urc; pub mod utils; diff --git a/pico/pico-lib/src/network.rs b/pico/pico-lib/src/network.rs index d652343..4b00f3a 100644 --- a/pico/pico-lib/src/network.rs +++ b/pico/pico-lib/src/network.rs @@ -145,10 +145,7 @@ pub enum SlowClockMode { #[cfg(test)] extern crate std; -pub async fn init_network( - client: &mut T, - pico: &mut U, -) { +pub async fn init(client: &mut T, pico: &mut U) { let mut registered = false; while !registered { loop { @@ -464,7 +461,7 @@ mod tests { .push_back(Ok("0,0,\"PANNON GSM\"".as_bytes())); // AT+COPS let mut pico = crate::at::tests::PicoMock::default(); - init_network(&mut client, &mut pico).await; + init(&mut client, &mut pico).await; assert_eq!(11, client.sent_commands.len()); assert_eq!("ATE0\r", client.sent_commands.get(0).unwrap()); assert_eq!("AT\r", client.sent_commands.get(1).unwrap()); diff --git a/pico/pico-lib/src/service.rs b/pico/pico-lib/src/service.rs new file mode 100644 index 0000000..edbe40e --- /dev/null +++ b/pico/pico-lib/src/service.rs @@ -0,0 +1,536 @@ +use core::cmp::{max, min}; + +use alloc::format; +use alloc::string::ToString; +use alloc::vec::Vec; +use atat::heapless::String; +use defmt::info; +use libm::pow; + +use crate::call::call_number; +use crate::location::Location; +use crate::poro::{ + CarLocation, ParkLocation, Position, Protector, ProtectorHuman, ProtectorMachine, ReceiverInfo, + Watcher, WatcherHuman, WatcherMachine, +}; +use crate::sms::{SmsStat, read_sms, send_sms}; +use crate::utils::{astring_to_string, get_distance_in_meters, is_distance_big_enough}; +use crate::{battery, call, gps, network, sms}; + +pub struct Configuration { + // The phone number is used for alerting + // - call at start up (charge -> device restart -> call) + // - call when the device is moved out from the parking zone + // - battery alert + // - SMS notifications + // - answer incoming call from this number + pub phone_number: String<30>, + // Password for the SMS commands + pub sms_password: String<30>, + + // The device wakes up every check_period_seconds and + // refreshes the device's location, battery, parking state, etc + pub service_enabled: bool, + // How long the to poll for location + pub locator_poll_count: u8, + // The period interval to run the service logic + pub check_period_seconds: u32, + + // Call the phone_number after boot + pub call_after_boot: bool, + // Debug park location updates, car theft etc (with the Watcher Application) + pub debug_alerts: bool, + // Send low battery alert + pub battery_alerts: bool, + // Detect parking + pub detect_parking: bool, + + // Keep only the newest N SMS message + pub keep_n_sms: u32, +} + +pub struct DeviceStatus { + pub last_big_location_change: i64, + pub location: Option, + pub park_location: Option, + pub battery: f32, + pub last_battery_alert: i64, +} + +pub struct Service { + // todo: persistence layer for cfg/status + pub cfg: Configuration, + pub status: DeviceStatus, +} + +const FIVE_MINUTES_IN_MILLIS: u64 = 5 * 60 * 1_000; +const TWENTY_FIVE_MINUTES_IN_MILLIS: u64 = 5 * FIVE_MINUTES_IN_MILLIS; +const MINIMUM_PARK_LOCATION_ACCURACY_IN_METERS: f64 = 150.0; + +impl Service { + pub async fn init( + &mut self, + client: &mut T, + pico: &mut U, + ) { + info!("######### CONFIGURATION #########"); + info!(" phone_number: {}", &self.cfg.phone_number); + info!(" sms_password: {}", &self.cfg.sms_password); + info!(""); + info!(" service_enabled: {}", self.cfg.service_enabled); + info!(" locator_poll_count: {}", &self.cfg.locator_poll_count); + info!(" check_period_seconds: {}", &self.cfg.check_period_seconds); + info!(""); + info!(" call_after_boot: {}", &self.cfg.call_after_boot); + info!(" debug_alerts: {}", &self.cfg.debug_alerts); + info!(" battery_alerts: {}", &self.cfg.battery_alerts); + info!(" detect_parking: {}", &self.cfg.detect_parking); + info!(""); + info!(" keep_n_sms: {}", &self.cfg.keep_n_sms); + info!("#################################"); + + // 3x long flash after boot + for _ in 0..3 { + pico.set_led_high(); + pico.sleep(500).await; + pico.set_led_low(); + pico.sleep(500).await; + } + + info!("Service: init network"); + network::init(client, pico).await; + info!("Service: init call"); + call::init(client, pico).await; + info!("Service: init sms"); + sms::init(client, pico).await; + info!("Service: init rtc time"); + let now = gps::get_gps_unix_timestamp_millis(client, pico, 30).await; + if now > 0 { + pico.set_rtc_time(now); + info!("Service: init rtc time succeeded"); + } else { + info!("Service: init rtc time failed"); + } + + // 10x short flash after init + for _ in 0..10 { + pico.set_led_high(); + pico.sleep(100).await; + pico.set_led_low(); + pico.sleep(100).await; + } + + if self.cfg.call_after_boot { + self.call_paired_phone(client, pico).await; + } + } + + pub async fn handle_incoming_call( + &mut self, + client: &mut T, + pico: &mut U, + phone_number: &String<30>, + ) { + if &self.cfg.phone_number == phone_number { + pico.sleep(2000).await; + call::answer_incoming_call(client, pico).await; + } else { + call::hangup_incoming_call(client, pico).await; + } + } + + pub async fn handle_sms( + &mut self, + client: &mut T, + pico: &mut U, + index: u32, + ) { + if let Some(sms) = read_sms(client, pico, index).await.ok() { + if sms.stat == SmsStat::ReceivedUnread { + let v: Vec<&str> = sms.message.split('/').collect(); + if v.len() != 3 { + return; + } + let prefix = v.get(0).unwrap(); + let command = v.get(1).unwrap(); + let password = v.get(2).unwrap(); + if self.cfg.sms_password != *password { + info!("invalid password"); + return; + } + + let w: Option = match *prefix { + "$tATA" => { + let p = WatcherHuman {}; + let w = p.parse(command.to_string()); + match w { + Ok(w) => Some(Watcher { + call: w.call, + refresh: w.refresh, + park: w.park, + receiver: Some(ReceiverInfo { + source: crate::poro::Source::SmsHuman, + phone_number: sms.phone_number.to_string(), + }), + service: w.service, + }), + Err(e) => { + info!("could not parse watcher human {}", e); + None + } + } + } + "$TATA" => { + let p = WatcherMachine {}; + let w = p.parse(command.to_string()); + match w { + Ok(w) => Some(Watcher { + call: w.call, + refresh: w.refresh, + park: w.park, + receiver: Some(ReceiverInfo { + source: crate::poro::Source::SmsMachine, + phone_number: sms.phone_number.to_string(), + }), + service: w.service, + }), + Err(e) => { + info!("could not parse watcher machine {}", e); + None + } + } + } + _ => { + info!("not a tATA command"); + None + } + }; + + if let Some(w) = w { + let receiver = w.receiver.unwrap(); + let phone_number = astring_to_string::<30>(receiver.phone_number.as_str()); + + if let Some(s) = w.service { + self.cfg.service_enabled = s.value; + } + + if let Some(r) = &w.refresh { + if r.value { + self.update_battery(client, pico).await; + self.refresh(client, pico).await; + } + } + + if let Some(p) = w.park { + if p.value { + if let Some(location) = &self.status.location { + self.save_park_location( + client, + pico, + &Location { + latitude: location.latitude, + longitude: location.longitude, + accuracy: location.accuracy, + unix_timestamp_millis: location.unix_timestamp_millis, + }, + ) + .await; + } else { + self.clear_park_location(client, pico).await; + } + } else { + self.clear_park_location(client, pico).await; + } + } + + if let Some(r) = w.refresh { + if r.value { + self.send_message(client, pico, &phone_number, &receiver.source, None) + .await; + } + } + + if let Some(c) = w.call { + if c.value { + call_number(client, pico, &phone_number, FIVE_MINUTES_IN_MILLIS).await; + } + } + } + } + } + } + + pub async fn update_battery( + &mut self, + client: &mut T, + pico: &mut U, + ) { + info!("Service: trying to update battery"); + if let Some(b) = battery::get_battery(client, pico).await { + self.status.battery = max(0u8, min(100u8, b.bcl)) as f32 / 100.0f32; + info!("Service: battery updated {}", self.status.battery); + + const BATTERY_LOW: f32 = 0.25; + const THREE_HOURS: i64 = 3 * 60 * 60 * 1000; + + if self.status.battery < BATTERY_LOW && self.cfg.battery_alerts { + let now = pico.rtc_now_millis(); + if (now - self.status.last_battery_alert) > THREE_HOURS { + self.status.last_battery_alert = now; + let message = format!("Battery alert {:.2} %!", self.status.battery); + send_sms( + client, + pico, + &self.cfg.phone_number, + &astring_to_string::<160>(&message), + ) + .await; + } + } + } + } + + pub async fn refresh( + &mut self, + client: &mut T, + pico: &mut U, + ) { + info!("Service: trying to update location"); + + // todo fallback to gsm position + let new_location = gps::get_gps_location(client, pico, self.cfg.locator_poll_count).await; + if new_location.is_none() { + return; + } + let mut new_location = new_location.unwrap(); + + info!("GPS location received {}", new_location); + + if (new_location.unix_timestamp_millis - pico.rtc_now_millis()).abs() > 60_000 { + pico.set_rtc_time(new_location.unix_timestamp_millis); + } + + let mut accuracy = new_location.accuracy; + // TODO: do we need this? (my old android code with 68th percentile) + // We define accuracy as the radius of 68% confidence. + accuracy = accuracy / 0.68; + // Unfortunately locations are not reliable when the car is in a garage. + // This math.pow will try to reduce false alarms. + // 10 meters -> 15~, 100 -> 200~, 3000 -> 10000~ + accuracy = pow(accuracy, 1.15); + new_location.accuracy = accuracy; + + info!("GPS location after accuracy adjusted {}", new_location); + + if let Some(p) = &self.status.park_location { + if is_distance_big_enough(&new_location, &p) { + // Car Theft detected + self.send_debug_message(client, pico, Some(crate::poro::Status::CarTheftDetected)) + .await; + self.clear_park_location(client, pico).await; + self.call_paired_phone(client, pico).await; + } else { + // Update park location (accuracy might have improved) + // + // Should not break the location based car theft detection when the car is moved slowly. + // The maximum amount of car movement in meters before the park location is not + // updated anymore is less then park_start_accuracy * 2. + // + // max_movement < park_start_accuracy * 2 + // + // input: + // M : minimum_park_location_accuracy / 2 + // X : park_start_accuracy + // + // output: + // sum X * 2 ^ (1-i), i=1 to log2(X / M) + // + // e.g: + // sum 600 * 2 ^ (1-i), i=1 to log2(600 / 75) + // + // http://www.wolframalpha.com/input/?i=sum+600+*+2+%5E+%281-i%29%2C+i%3D1+to+log2%28600+%2F+75%29 + let distance = get_distance_in_meters( + new_location.latitude, + new_location.longitude, + p.latitude, + p.longitude, + ); + if distance <= p.accuracy { + if f64::max( + new_location.accuracy * 2f64, + MINIMUM_PARK_LOCATION_ACCURACY_IN_METERS, + ) < p.accuracy + { + self.save_park_location(client, pico, &new_location).await; + self.send_debug_message( + client, + pico, + Some(crate::poro::Status::ParkingUpdated), + ) + .await; + } + } + } + } else if self.cfg.detect_parking { + if let Some(last_location) = &self.status.location { + if is_distance_big_enough(&new_location, last_location) { + self.status.last_big_location_change = new_location.unix_timestamp_millis; + } else if new_location.unix_timestamp_millis - self.status.last_big_location_change + > TWENTY_FIVE_MINUTES_IN_MILLIS as i64 + { + // Parking detected + self.save_park_location(client, pico, &new_location).await; + self.send_debug_message( + client, + pico, + Some(crate::poro::Status::ParkingDetected), + ) + .await; + } + } else { + self.status.last_big_location_change = new_location.unix_timestamp_millis; + } + } + + self.save_location(client, pico, &new_location).await; + } + + async fn call_paired_phone( + &mut self, + client: &mut T, + pico: &mut U, + ) { + info!("Service: call paired phone='{}'", &self.cfg.phone_number); + if self.cfg.phone_number.len() > 0 { + call_number(client, pico, &self.cfg.phone_number, FIVE_MINUTES_IN_MILLIS).await; + } + } + + async fn send_debug_message( + &mut self, + client: &mut T, + pico: &mut U, + status: Option, + ) { + info!( + "Service: send debug message dbg='{}' phone='{}' status='{}'", + self.cfg.debug_alerts, &self.cfg.phone_number, status + ); + if self.cfg.debug_alerts { + if self.cfg.phone_number.len() > 0 { + let phone_number = self.cfg.phone_number.clone(); + self.send_message( + client, + pico, + &phone_number, + &crate::poro::Source::SmsMachine, + status, + ) + .await; + } + } + } + + async fn send_message( + &mut self, + client: &mut T, + pico: &mut U, + phone_number: &String<30>, + receiver_type: &crate::poro::Source, + status: Option, + ) { + let car_location = match self.status.location.as_ref() { + Some(l) => Some(CarLocation { + position: Position { + latitude: l.latitude, + longitude: l.longitude, + }, + accuracy: l.accuracy as f32, + battery: self.status.battery, + timestamp: l.unix_timestamp_millis, + }), + None => None, + }; + + let park_location = match self.status.park_location.as_ref() { + Some(l) => Some(ParkLocation { + position: Position { + latitude: l.latitude, + longitude: l.longitude, + }, + accuracy: l.accuracy as f32, + }), + None => None, + }; + + let protector = Protector { + car_location: car_location, + park_location: park_location, + status: status, + service: Some(crate::poro::Service { + value: self.cfg.service_enabled, + }), + }; + + let message = match receiver_type { + crate::poro::Source::SmsHuman => { + let p = ProtectorHuman {}; + p.dump(&protector) + } + crate::poro::Source::SmsMachine => { + let p = ProtectorMachine {}; + let dumped = p.dump(&protector); + let mut tata_response = "$tATA/".to_string(); + let _ = tata_response.push_str(dumped.as_str()); + tata_response + } + _ => "".to_string(), + }; + + if message.len() > 0 { + send_sms( + client, + pico, + &phone_number, + &astring_to_string::<160>(message.as_str()), + ) + .await + } + } + + async fn save_location( + &mut self, + _client: &mut T, + _pico: &mut U, + location: &Location, + ) { + self.status.location = Some(Location { + latitude: location.latitude, + longitude: location.longitude, + accuracy: location.accuracy, + unix_timestamp_millis: location.unix_timestamp_millis, + }); + } + + async fn save_park_location( + &mut self, + _client: &mut T, + _pico: &mut U, + location: &Location, + ) { + self.status.park_location = Some(Location { + latitude: location.latitude, + longitude: location.longitude, + accuracy: f64::max(location.accuracy, MINIMUM_PARK_LOCATION_ACCURACY_IN_METERS), + unix_timestamp_millis: location.unix_timestamp_millis, + }); + } + + async fn clear_park_location( + &mut self, + _client: &mut T, + _pico: &mut U, + ) { + self.status.park_location = None + } +} diff --git a/pico/pico-lib/src/utils.rs b/pico/pico-lib/src/utils.rs index 74db9aa..8f54bdf 100644 --- a/pico/pico-lib/src/utils.rs +++ b/pico/pico-lib/src/utils.rs @@ -8,6 +8,8 @@ use alloc::collections::vec_deque::VecDeque; use alloc::string::String; use libm::{asin, cos, pow, sin, sqrt}; +use crate::location::Location; + // https://stackoverflow.com/questions/27928/calculate-distance-between-two-latitude-longitude-points-haversine-formula pub fn get_distance_in_meters(lat1: f64, lon1: f64, lat2: f64, lon2: f64) -> f64 { let earth_radius_in_meters = 6371000f64; @@ -20,6 +22,11 @@ pub fn get_distance_in_meters(lat1: f64, lon1: f64, lat2: f64, lon2: f64) -> f64 return earth_radius_in_meters * c; } +pub fn is_distance_big_enough(l1: &Location, l2: &Location) -> bool { + return get_distance_in_meters(l1.latitude, l1.longitude, l2.latitude, l2.longitude) + > (l1.accuracy + l2.accuracy); +} + // https://gis.stackexchange.com/questions/111004/translating-hdop-pdop-and-vdop-to-metric-accuracy-from-given-nmea-strings pub fn estimate_gps_accuracy(pdop: f64) -> f64 { // Accuracy 2.5m CEP (circular error probable)