diff --git a/.clippy.toml b/.clippy.toml index 3e969e0..a8f21f2 100644 --- a/.clippy.toml +++ b/.clippy.toml @@ -1,2 +1,2 @@ stack-size-threshold = 1024 -doc-valid-idents = ["WiFi", "GeoNetworking", "QoS", ".."] +doc-valid-idents = ["WiFi", "GeoNetworking", "QoS", "LiPo", ".."] diff --git a/Readme.md b/Readme.md index ccbedb0..2be427e 100644 --- a/Readme.md +++ b/Readme.md @@ -146,7 +146,10 @@ Images are loaded as TGA assets, but shall not have an alpha channel. You can convert them using [ImageMagick](https://imagemagick.org) like this: ```shell # resize to 25px height, adding a black background to the white symbol and vice versa -magick Sinnbild_Radfahrer_StVO_1992-200px-white.png -resize x25 -background black -alpha background -flatten -alpha off -verbose Sinnbild_Radfahrer_StVO_1992-25px-white.tga -magick Sinnbild_Radfahrer_StVO_1992-200px-black.png -resize x25 -background white -alpha background -flatten -alpha off -verbose Sinnbild_Radfahrer_StVO_1992-25px-black.tga -``` +magick Sinnbild_Radfahrer_StVO_1992-200px-white.png -resize x25 -background black -alpha background -flatten -alpha off -colorspace Gray -verbose Sinnbild_Radfahrer_StVO_1992-25px-white.tga +magick Sinnbild_Radfahrer_StVO_1992-200px-black.png -resize x25 -background white -alpha background -flatten -alpha off -colorspace Gray -verbose Sinnbild_Radfahrer_StVO_1992-25px-black.tga +magick Stationary_Emergency_Vehicle-140x105.svg -resize 70 -background black -alpha background -flatten -alpha off -colorspace Gray -verbose Stationary_Emergency_Vehicle-70px.tga +magick Stationary_Vehicle-140x70.svg -resize 80 -background black -alpha background -flatten -alpha off -colorspace Gray -verbose Stationary_Vehicle-80px.tga +magick Work_Zone-140x95.svg -resize 80 -background black -alpha background -flatten -alpha off -colorspace Gray -verbose Work_Zone-80px.tga +``` diff --git a/graphics/Stationary_Emergency_Vehicle-140x105.svg b/graphics/Stationary_Emergency_Vehicle-140x105.svg new file mode 100644 index 0000000..f5b5aa0 --- /dev/null +++ b/graphics/Stationary_Emergency_Vehicle-140x105.svg @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/graphics/Stationary_Emergency_Vehicle-70px.tga b/graphics/Stationary_Emergency_Vehicle-70px.tga new file mode 100644 index 0000000..3076adb Binary files /dev/null and b/graphics/Stationary_Emergency_Vehicle-70px.tga differ diff --git a/graphics/Stationary_Vehicle-140x70.svg b/graphics/Stationary_Vehicle-140x70.svg new file mode 100644 index 0000000..3ff1cd7 --- /dev/null +++ b/graphics/Stationary_Vehicle-140x70.svg @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/graphics/Stationary_Vehicle-80px.tga b/graphics/Stationary_Vehicle-80px.tga new file mode 100644 index 0000000..475ef75 Binary files /dev/null and b/graphics/Stationary_Vehicle-80px.tga differ diff --git a/graphics/Work_Zone-140x95.svg b/graphics/Work_Zone-140x95.svg new file mode 100644 index 0000000..bbbe6fb --- /dev/null +++ b/graphics/Work_Zone-140x95.svg @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/graphics/Work_Zone-80px.tga b/graphics/Work_Zone-80px.tga new file mode 100644 index 0000000..a397e21 Binary files /dev/null and b/graphics/Work_Zone-80px.tga differ diff --git a/run-standalone-denm.sh b/run-standalone-denm.sh new file mode 100755 index 0000000..70a0905 --- /dev/null +++ b/run-standalone-denm.sh @@ -0,0 +1,6 @@ +#!/bin/sh + +## Builds common config for standalone device +## -> enable DENM with GNSS and a screen (no GLOSA) + +cargo run --release --no-default-features -F esp,screen,gnss_ubx,denm,ble diff --git a/run-standalone-glosa+denm.sh b/run-standalone-glosa+denm.sh new file mode 100755 index 0000000..fa0f975 --- /dev/null +++ b/run-standalone-glosa+denm.sh @@ -0,0 +1,6 @@ +#!/bin/sh + +## Builds common config for standalone device +## -> enable SPAT/ GLOSA and DENM icons with GNSS and a screen and BLE + +cargo run --release -F denm,ble diff --git a/src/applogic/denm.rs b/src/applogic/denm.rs index 9d46dbb..d4a5516 100644 --- a/src/applogic/denm.rs +++ b/src/applogic/denm.rs @@ -1,13 +1,14 @@ //! C-ITS DENM Evaluation +use c_its_parser::standards::cdd_2_2_1::etsi_its_cdd; use c_its_parser::standards::denm_2_2_1::denm_pdu_description; use log::info; -pub fn handle_denm(etsi: &denm_pdu_description::DENM) { +pub fn handle_denm(etsi: &denm_pdu_description::DENM) -> Option { let mgmt = &etsi.denm.management; if mgmt.termination.is_some() { - return; + return None; } if let (Some(sit), Some(loc)) = (&etsi.denm.situation, &etsi.denm.location) { @@ -38,7 +39,11 @@ pub fn handle_denm(etsi: &denm_pdu_description::DENM) { "DENN: {station_id}/{seq_num} CC {cc}/{scc} at {lat_deg:.3}°N, {lon_deg:.3}°E until {end_time:?}" ); } + + Some(sit.event_type.cc_and_scc.clone()) } else { info!("DENM: One of situation or location container not available"); + + None } } diff --git a/src/ble.rs b/src/ble.rs index a460d4e..8fd2e65 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -23,6 +23,8 @@ pub static TX_DATA: Mutex>>> = pub static RX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); pub enum BleSendable { + BatteryLevel(u8), + #[cfg(feature = "cam")] CamRaw(io::msg::RawMsgRx), #[cfg(feature = "cam")] @@ -42,6 +44,16 @@ pub enum BleSendable { #[gatt_server] struct Server { c_its_events: CITSEvents, + battery_service: BatteryService, +} + +#[gatt_service(uuid = service::BATTERY)] +struct BatteryService { + /// Battery Level + #[descriptor(uuid = descriptors::VALID_RANGE, read, value = [0, 100])] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "main", read, value = "Battery Level", type = &'static str)] + #[characteristic(uuid = characteristic::BATTERY_LEVEL, read, notify, value = 0)] + level: u8, } #[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")] @@ -110,6 +122,7 @@ pub async fn run( CONTROLLER_SLOTS, >, name: alloc::string::String, + short_name: alloc::string::String, ) { let mut resources: HostResources = HostResources::new(); @@ -120,8 +133,9 @@ pub async fn run( .. } = stack.build(); + info!("BLE name: {name} (short: {short_name})"); info!( - "Starting advertising and GATT service for {}", + "Starting advertising and GATT service for {} as '{name}'/ '{short_name}'", crate::util::get_rx_feature_list().join(",") ); let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig { @@ -132,7 +146,7 @@ pub async fn run( let _ = embassy_futures::join::join(ble_task(runner), async { loop { - match advertise(&mut peripheral, &server).await { + match advertise(&mut peripheral, &server, &short_name).await { Ok(conn) => { info!("BLE: New client connected"); @@ -176,6 +190,8 @@ pub fn update(payload: BleSendable) { async fn advertise<'values, 'server, C: Controller>( peripheral: &mut Peripheral<'values, C, DefaultPacketPool>, server: &'server Server<'values>, + // name in advertisement, max. 12 characters + short_name: &'values str, ) -> Result, BleHostError> { let mut advertiser_data = [0; 35]; let service_uuid = Uuid::from(0xc0b7_0000_d4f4_4000_ada8_f99a_02ee_315c_u128) @@ -184,8 +200,9 @@ async fn advertise<'values, 'server, C: Controller>( .unwrap(); let len = AdStructure::encode_slice( &[ - AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), - AdStructure::ServiceUuids128(&[service_uuid]), + AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), // 2+ 1 byte + AdStructure::ServiceUuids128(&[service_uuid]), // 2+16 byte = 21 + AdStructure::CompleteLocalName(short_name.as_bytes()), // 2+ 12 byte = 35 ], &mut advertiser_data[..], )?; @@ -286,6 +303,12 @@ async fn publishing_task(server: &Server<'_>, conn: &GattConnecti }); match new_data { + Some(BleSendable::BatteryLevel(value)) => { + if let Err(err) = server.battery_service.level.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new battery level: {err:?}"); + } + } + #[cfg(feature = "cam")] Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() { Ok(value) => { diff --git a/src/main.rs b/src/main.rs index 5c23fe6..e99012e 100644 --- a/src/main.rs +++ b/src/main.rs @@ -59,7 +59,9 @@ mod screen; #[cfg(feature = "gnss_ubx")] mod ubx; mod util; +mod xiao; +const BAT_UPDATE_INTERVAL: esp_hal::time::Duration = esp_hal::time::Duration::from_secs(30); const WIFI_CHANNEL: radio::Channel = 180; const WIFI_TXPOWER: i8 = 10; // dBm, 2..20 #[cfg(feature = "spat")] @@ -144,6 +146,10 @@ async fn main(spawner: Spawner) -> ! { info!("Embassy initialized!"); + // Setup battery voltage readout (GPIO6 as input and GPIO26 as enable) + let mut battery = xiao::Battery::new(peripherals.ADC1, peripherals.GPIO6, peripherals.GPIO26); + // TODO: expose via BLE as battery service! + // Setup ST7789 screen // SCL: D8/ GPIO8 (SPI_SCK) // SDA: D10/ GPIO10 (SPI_MOSI) @@ -227,7 +233,8 @@ async fn main(spawner: Spawner) -> ! { serial[1], serial[2] ); - spawner.spawn(ble::run(controller, name).expect("Failed to spawn BLE task")); + let short_name = alloc::format!("V2X {:02x}{:02x}{:02x}", serial[0], serial[1], serial[2]); // max 8 chars! -> 10 chars + spawner.spawn(ble::run(controller, name, short_name).expect("Failed to spawn BLE task")); } // Setup GNSS: @@ -272,6 +279,10 @@ async fn main(spawner: Spawner) -> ! { #[cfg(feature = "_gnss")] info!("GNSS task started, waiting for GNSS fix..."); + // Setup main loop + info!("Battery: {} %", battery.read_percent()); + let mut last_bat_update = esp_hal::time::Instant::now(); + #[cfg(any(feature = "cam_tx", feature = "uart"))] let (mut seq_no, mac_bytes) = { // unwrap is fine since [`esp_hal::efuse::MacAddress`] is just an `[u8; 6]` @@ -389,7 +400,16 @@ async fn main(spawner: Spawner) -> ! { } // update GNSS HB on screen - let _ = screen.update_gnss(pvt); + #[cfg(feature = "screen")] + { + let _ = screen.update_gnss(pvt); + + // revert to waiting for DENMs, if SPAT is not enabled + #[cfg(all(not(feature = "spat"), feature = "denm"))] + let _ = screen + .update(screen::ScreenState::DenmWaiting()) + .inspect_err(log_screen_error); + } } ublox::proto27::PacketRef::AckNak(nak) => { error!("UBX: {nak:?}"); @@ -579,7 +599,15 @@ async fn main(spawner: Spawner) -> ! { transport: _, ref etsi, } => { - applogic::denm::handle_denm(etsi); + match applogic::denm::handle_denm(etsi) { + #[cfg(feature = "screen")] + Some(cc) => { + let _ = screen + .update(screen::ScreenState::DenmWarning(cc)) + .inspect_err(log_screen_error); + } + _ => {} + } // Send data to BLE thread #[cfg(feature = "ble")] @@ -614,6 +642,17 @@ async fn main(spawner: Spawner) -> ! { // maintenance state.prune(); + + let cpu_now = esp_hal::time::Instant::now(); + if last_bat_update + BAT_UPDATE_INTERVAL < cpu_now { + let bat_percent = battery.read_percent(); + info!("Battery: {bat_percent} %"); + + #[cfg(feature = "ble")] + ble::update(ble::BleSendable::BatteryLevel(bat_percent)); + + last_bat_update = cpu_now; + } } } diff --git a/src/screen.rs b/src/screen.rs index 32bcfc7..4f9d01d 100644 --- a/src/screen.rs +++ b/src/screen.rs @@ -1,11 +1,16 @@ //! Screen Output +#[cfg(feature = "denm")] +use c_its_parser::standards::cdd_2_2_1; use embedded_graphics::prelude::*; use embedded_graphics::{geometry, mono_font, pixelcolor, primitives, text}; #[cfg(feature = "spat")] use crate::applogic; +#[cfg(feature = "denm")] +const DENM_MIN_DISPLAY_TIME: esp_hal::time::Duration = esp_hal::time::Duration::from_secs(3); + pub struct DisplaySize { width: u16, height: u16, @@ -24,10 +29,20 @@ where prev_state: ScreenState, hb_state: bool, gnss_hb_state: bool, + #[cfg(feature = "denm")] + denm_display_start_time: esp_hal::time::Instant, // assets #[cfg(feature = "spat")] - bike_image: tinytga::Tga<'static, pixelcolor::Rgb565>, + bike_image: tinytga::Tga<'static, pixelcolor::Rgb565>, // 45x25px + // #[cfg(feature = "denm")] + // warn_icon: tinytga::Tga<'static, pixelcolor::Rgb565>, // 200x153 px + #[cfg(feature = "denm")] + sv_insert: tinytga::Tga<'static, pixelcolor::Rgb565>, // 80px wide (original 140x70) + #[cfg(feature = "denm")] + ev_insert: tinytga::Tga<'static, pixelcolor::Rgb565>, // 60px wide (original 140x105) + #[cfg(feature = "denm")] + rww_insert: tinytga::Tga<'static, pixelcolor::Rgb565>, // 80px wide (original 140x95) } #[derive(Debug, Default, PartialEq, Eq)] @@ -43,6 +58,10 @@ pub enum ScreenState { GlosaFound(applogic::GlosaFound), #[cfg(feature = "spat")] GlosaLocked(applogic::GlosaSignalInfo), + #[cfg(all(not(feature = "spat"), feature = "denm"))] + DenmWaiting(), + #[cfg(feature = "denm")] + DenmWarning(cdd_2_2_1::etsi_its_cdd::CauseCodeChoice), } #[cfg(feature = "spat")] @@ -101,9 +120,23 @@ where pub fn new(target: D, display_size: DisplaySize) -> Self { #[cfg(feature = "spat")] - let data = include_bytes!("../graphics/Sinnbild_Radfahrer_StVO_1992-25px-white.tga"); - #[cfg(feature = "spat")] - let bike_image = tinytga::Tga::from_slice(data).expect("Failed to parse TGA image"); + let bike_image = tinytga::Tga::from_slice(include_bytes!( + "../graphics/Sinnbild_Radfahrer_StVO_1992-25px-white.tga" + )) + .expect("Failed to parse TGA image"); + + #[cfg(feature = "denm")] + let sv_insert = + tinytga::Tga::from_slice(include_bytes!("../graphics/Stationary_Vehicle-80px.tga")) + .expect("Failed to parse TGA image"); + #[cfg(feature = "denm")] + let ev_insert = tinytga::Tga::from_slice(include_bytes!( + "../graphics/Stationary_Emergency_Vehicle-70px.tga" + )) + .expect("Failed to parse TGA image"); + #[cfg(feature = "denm")] + let rww_insert = tinytga::Tga::from_slice(include_bytes!("../graphics/Work_Zone-80px.tga")) + .expect("Failed to parse TGA image"); Self { target, @@ -111,15 +144,53 @@ where prev_state: ScreenState::Uninitialized, hb_state: false, gnss_hb_state: false, + #[cfg(feature = "denm")] + denm_display_start_time: esp_hal::time::Instant::EPOCH, #[cfg(feature = "spat")] bike_image, + // #[cfg(feature = "denm")] + // warn_icon, + #[cfg(feature = "denm")] + sv_insert, + #[cfg(feature = "denm")] + ev_insert, + #[cfg(feature = "denm")] + rww_insert, } } pub fn update(&mut self, state: ScreenState) -> Result<(), D::Error> { + #[cfg(feature = "denm")] + let now = esp_hal::time::Instant::now(); + + #[cfg(feature = "denm")] + // when a DENM was shown before, make sure it's shown for a minimum time + if let ScreenState::DenmWarning(_) = self.prev_state { + use core::ops::Sub; + + // update DENM display, if there are updates + if let ScreenState::DenmWarning(cause_code_choice) = &state { + // TODO: find a way to immediately turn off the DENM display when the DENM is over, but keep it for a certain time initially + self.denm_display_start_time = now; + + if self.prev_state != state { + self.draw_denm_warning(cause_code_choice)?; + self.prev_state = state; + + return Ok(()); + } + } + + // otherwise early return when timer isn't expired yet + if now.sub(self.denm_display_start_time) < DENM_MIN_DISPLAY_TIME { + return Ok(()); + } + } + if self.prev_state != state { match &state { ScreenState::Uninitialized | ScreenState::Initial => self.draw_slash_screen()?, + #[cfg(feature = "spat")] ScreenState::GlosaSearching(_glosa_searching) => self.draw_glosa_searching()?, #[cfg(feature = "spat")] @@ -136,10 +207,18 @@ where self.draw_glosa(glosa_data, prev_glosa_data)?; } ScreenState::Error(msg) => self.draw_message(msg)?, + #[cfg(all(not(feature = "spat"), feature = "denm"))] + ScreenState::DenmWaiting() => self.draw_denm_waiting()?, + #[cfg(feature = "denm")] + ScreenState::DenmWarning(cause_code_choice) => { + self.denm_display_start_time = now; + self.draw_denm_warning(cause_code_choice)?; + } } } // Update indicator/ heartbeat + #[cfg(feature = "spat")] { let update_ind_dia = 10; let circle_x_pos = @@ -477,6 +556,100 @@ where Ok(()) } + #[cfg(all(not(feature = "spat"), feature = "denm"))] + fn draw_denm_waiting(&mut self) -> Result<(), D::Error> { + // clear whole display and draw text + self.clear()?; + + text::Text::with_alignment( + "consider\ninnovation", + geometry::Point::new(i32::from(self.size.width) / 2, 40 + self.size.offset_top), + Self::MID_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + text::Text::with_alignment( + "Waiting\nfor DENM...", + geometry::Point::new(i32::from(self.size.width) / 2, 130 + self.size.offset_top), + Self::DEFAULT_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + Ok(()) + } + + #[cfg(feature = "denm")] + pub fn draw_denm_warning( + &mut self, + cc: &cdd_2_2_1::etsi_its_cdd::CauseCodeChoice, + ) -> Result<(), D::Error> { + // clear whole display and draw image + self.clear()?; + + let centerline = i32::from(self.size.width) / 2; + let sign_top = self.size.offset_top + 40; + + // build triangle shape + { + let point1 = geometry::Point::new(centerline, sign_top); + let point2 = geometry::Point::new(centerline - 100, sign_top + 150); + let point3 = geometry::Point::new(centerline + 100, sign_top + 150); + let mut line_style = + primitives::PrimitiveStyle::with_stroke(pixelcolor::Rgb565::CSS_RED, 10); + line_style.fill_color = Some(pixelcolor::Rgb565::WHITE); + + primitives::Triangle::new(point1, point2, point3) + .into_styled(line_style) + .draw(&mut self.target)?; + } + + // add icon inside the triangle + match cc { + cdd_2_2_1::etsi_its_cdd::CauseCodeChoice::slowVehicle26(_) + | cdd_2_2_1::etsi_its_cdd::CauseCodeChoice::stationaryVehicle94(_) => { + embedded_graphics::image::Image::new( + &self.sv_insert, + geometry::Point::new(centerline - (80 / 2), sign_top + 90), + ) + .draw(&mut self.target)?; + } + + cdd_2_2_1::etsi_its_cdd::CauseCodeChoice::rescueAndRecoveryWorkInProgress15(_) + | cdd_2_2_1::etsi_its_cdd::CauseCodeChoice::emergencyVehicleApproaching95(_) => { + embedded_graphics::image::Image::new( + &self.ev_insert, + geometry::Point::new(centerline - (70 / 2), sign_top + 85), + ) + .draw(&mut self.target)?; + } + + cdd_2_2_1::etsi_its_cdd::CauseCodeChoice::roadworks3(_) => { + embedded_graphics::image::Image::new( + &self.rww_insert, + geometry::Point::new(centerline - (80 / 2), sign_top + 85), + ) + .draw(&mut self.target)?; + } + _ => {} + } + + let (cc, scc) = cc.to_u8_tuple(); + text::Text::with_alignment( + &alloc::format!("DENM CC {cc}/{scc}"), + geometry::Point::new( + i32::from(self.size.width) / 2, + sign_top + 150 + (10 / 2) + 30, + ), + Self::MID_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; + + Ok(()) + } + #[allow(unused)] fn draw_slash_screen(&mut self) -> Result<(), D::Error> { self.clear_all()?; @@ -519,6 +692,13 @@ where ) .draw(&mut self.target)?; } else { + text::Text::with_alignment( + "consider\ninnovation", + geometry::Point::new(centerline, 40 + self.size.offset_top), + Self::MID_TEXT_STYLE, + text::Alignment::Center, + ) + .draw(&mut self.target)?; text::Text::with_alignment( "C-ITS Companion", geometry::Point::new(centerline, 130 + self.size.offset_top), diff --git a/src/xiao.rs b/src/xiao.rs new file mode 100644 index 0000000..6cbff88 --- /dev/null +++ b/src/xiao.rs @@ -0,0 +1,85 @@ +//! XIAO ESP32-C5 Helpers + +use esp_hal::analog::adc; +use esp_hal::gpio; +use log::debug; + +type AdcPeripheral<'a> = esp_hal::peripherals::ADC1<'a>; + +pub struct Battery<'a, IoAdc> +where + IoAdc: adc::AdcChannel + gpio::AnalogPin, +{ + // using `AdcCalCurve` here gives readings in millivolts + adc_pin: adc::AdcPin, adc::AdcCalCurve>>, + adc: adc::Adc<'a, AdcPeripheral<'a>, esp_hal::Blocking>, +} + +impl<'a, IoAdc> Battery<'a, IoAdc> +where + IoAdc: adc::AdcChannel + gpio::AnalogPin, +{ + pub fn new( + adc_periph: AdcPeripheral<'a>, + adc_pin: IoAdc, + enable_pin: IoEnable, + ) -> Self { + // enable battery voltage readout + let _ = gpio::Output::new(enable_pin, gpio::Level::High, gpio::OutputConfig::default()); + + // configure ADC + let mut adc_config = adc::AdcConfig::new(); + let adc_pin = adc_config.enable_pin_with_cal(adc_pin, adc::Attenuation::_11dB); + let adc = adc::Adc::new(adc_periph, adc_config); + + Self { adc_pin, adc } + } + + /// Reads from ADC and returns battery voltage in millivolts + pub fn read_millivolts(&mut self) -> u16 { + // average over 16 samples + let mut accu = 0u16; + for _ in 0..16 { + let val = loop { + match self.adc.read_oneshot(&mut self.adc_pin) { + Ok(value) => break value, + Err(err) => { + debug!("Failed to read from ADC: {err:?}") + } + } + }; + + accu += val; + } + let adc_val = accu / 16; + + // convert to battery voltage: 1:2 voltage divider + adc_val * 2 + } + + /// Returns estimated battery charge state in percent + pub fn read_percent(&mut self) -> u8 { + Self::millivolts_to_percent(self.read_millivolts()) + } + + /// Converts single cell LiPo battery voltage to charge state in percent + /// + /// Note: The implementation is a drastically simplified linear estimation. + /// This is known to be insufficient for an actual state of charge, but should be fine for now. + pub fn millivolts_to_percent(millivolt: u16) -> u8 { + let (full_mv, empty_mv) = (4200, 3270); // 100% to 0% SoC + + if millivolt < empty_mv { + 0 + } else if millivolt >= full_mv { + 100 + } else { + let percent = i32::from(millivolt - empty_mv) * 100 / i32::from(full_mv - empty_mv); + + #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] + let result = percent as u8; + + result + } + } +}