WIP ble: Add device information service

TODO: Determine "git describe" size

.clippy.toml

@@ -1,2 +1,2 @@
 stack-size-threshold = 1024
-doc-valid-idents = ["WiFi", "GeoNetworking", "QoS", ".."]
+doc-valid-idents = ["WiFi", "GeoNetworking", "QoS", "LiPo", ".."]

build.rs

@@ -17,6 +17,21 @@ fn main() -> std::io::Result<()> {
         .compile_well_known_types()
         .compile_protos(&["docs/c_its-messages.proto"], &["docs/"])?;
 
+    // get git revision
+    println!("cargo:rustc-rerun-if-changed=.git/HEAD");
+
+    println!(
+        "cargo:rustc-env=GIT_REVISION={}",
+        String::from_utf8(
+            std::process::Command::new("git")
+                .args(&["describe", "--always"])
+                .output()
+                .unwrap()
+                .stdout,
+        )
+        .unwrap()
+    );
+
     Ok(())
 }
 

src/ble.rs

@@ -23,6 +23,8 @@ pub static TX_DATA: Mutex<RefCell<Option<ArrayQueue<BleSendable>>>> =
 pub static RX_DATA: Mutex<RefCell<Option<io::msg::PositionState>>> = Mutex::new(RefCell::new(None));
 
 pub enum BleSendable {
+    BatteryLevel(u8),
+
     #[cfg(feature = "cam")]
     CamRaw(io::msg::RawMsgRx),
     #[cfg(feature = "cam")]
@@ -39,9 +41,83 @@ pub enum BleSendable {
     SpatemRaw(io::msg::RawMsgRx),
 }
 
+pub const DIS_SERIAL_MAX_LEN: usize = 17;
+pub const DIS_FW_REVISION_MAX_LEN: usize = 16; // TODO: what do we need?
+pub const DIS_SW_REVISION_MAX_LEN: usize = 66; // max length of get_feature_list, joined with comma
+
+pub struct DeviceInfo {
+    /// WiFi MAC
+    pub serial_number: [u8; DIS_SERIAL_MAX_LEN],
+    /// git tag/ revision
+    pub fw_revision: [u8; DIS_FW_REVISION_MAX_LEN],
+    /// used for feature flags
+    pub sw_revision: [u8; DIS_SW_REVISION_MAX_LEN],
+}
+
+impl DeviceInfo {
+    pub fn new(serial_number: &str, fw_revision: &str, sw_revision: &str) -> Self {
+        Self {
+            serial_number: string_as_padded_buf(serial_number, DIS_SERIAL_MAX_LEN)
+                .try_into()
+                .unwrap(),
+            fw_revision: string_as_padded_buf(fw_revision, DIS_FW_REVISION_MAX_LEN)
+                .try_into()
+                .unwrap(),
+            sw_revision: string_as_padded_buf(sw_revision, DIS_SW_REVISION_MAX_LEN)
+                .try_into()
+                .unwrap(),
+        }
+    }
+}
+
+pub fn string_as_padded_buf(value: &str, length: usize) -> alloc::vec::Vec<u8> {
+    let buf = value.as_bytes();
+    let buf_len = buf.len();
+
+    // cut buffer at maximum length or pad with whitespace
+    if buf_len > length {
+        buf[..length].to_vec()
+    } else {
+        let padding_len = length - buf_len;
+        let padding = alloc::vec![b' '; padding_len];
+
+        [buf, &padding].concat()
+    }
+}
+
 #[gatt_server]
 struct Server {
     c_its_events: CITSEvents,
+    battery_service: BatteryService,
+    device_info_service: DeviceInfoService,
+}
+
+#[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 = service::DEVICE_INFORMATION)]
+struct DeviceInfoService {
+    /// Manufacturer name
+    #[characteristic(uuid = characteristic::MANUFACTURER_NAME_STRING, read, value = "jtbx")]
+    mfg_name: &'static str,
+    /// Model number
+    #[characteristic(uuid = characteristic::MODEL_NUMBER_STRING, read, value = "ESP32-C5 C-ITS Companion")]
+    model_number: &'static str,
+    // Serial Number String
+    #[characteristic(uuid = characteristic::SERIAL_NUMBER_STRING, read, value = [0u8; DIS_SERIAL_MAX_LEN])]
+    serial_number: [u8; DIS_SERIAL_MAX_LEN],
+    // Firmware Revision String
+    #[characteristic(uuid = characteristic::FIRMWARE_REVISION_STRING, read, value = [0u8; DIS_FW_REVISION_MAX_LEN])]
+    fw_revision: [u8; DIS_FW_REVISION_MAX_LEN],
+    // Software Revision String
+    #[characteristic(uuid = characteristic::SOFTWARE_REVISION_STRING, read, value = [0u8; DIS_SW_REVISION_MAX_LEN])]
+    sw_revision: [u8; DIS_SW_REVISION_MAX_LEN],
 }
 
 #[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")]
@@ -111,6 +187,7 @@ pub async fn run(
     >,
     name: alloc::string::String,
     short_name: alloc::string::String,
+    device_info: DeviceInfo,
 ) {
     let mut resources: HostResources<DefaultPacketPool, CONNECTIONS_MAX, L2CAP_CHANNELS_MAX> =
         HostResources::new();
@@ -132,10 +209,28 @@ pub async fn run(
     }))
     .unwrap();
 
+    // set "dynamic" device info data
+    let _ = server
+        .device_info_service
+        .serial_number
+        .set(&server, &device_info.serial_number);
+    let _ = server
+        .device_info_service
+        .fw_revision
+        .set(&server, &device_info.fw_revision);
+    let _ = server
+        .device_info_service
+        .sw_revision
+        .set(&server, &device_info.sw_revision);
+
+    // advertise and run update logic
     let _ = embassy_futures::join::join(ble_task(runner), async {
         loop {
-            match advertise(&mut peripheral, &server, &short_name).await {
+            // run advertisement and set data until a client connects
-                Ok(conn) => {
+            let a = advertise(&mut peripheral, &server, &short_name);
+            let b = data_update_task(&server);
+            match embassy_futures::select::select(a, b).await {
+                embassy_futures::select::Either::First(Ok(conn)) => {
                     info!("BLE: New client connected");
 
                     // set up tasks when the connection is established to a central, so they don't run when no one is connected.
@@ -150,15 +245,17 @@ pub async fn run(
                         let _ = value_ref.replace(false);
                     });
                 }
-                Err(e) => {
+                embassy_futures::select::Either::First(Err(e)) => {
                     panic!("BLE ADV: fatal error: {e:?}");
                 }
+                _ => {}
             }
         }
     })
     .await;
 }
 
+/// Queues data for BLE notification (only when client is connected)
 pub fn update(payload: BleSendable) {
     critical_section::with(|cs| {
         // only add to queue when someone is connected
@@ -174,6 +271,24 @@ pub fn update(payload: BleSendable) {
     });
 }
 
+/// Queues data for characteristic update (may notify a connected client)
+///
+/// This can be used instead of `update()`, but will always write the data to the characteristic even when no client is connected.
+/// So a newly connected client will be able to read an up to date value at connection and not just when new data was received.
+///
+/// This is useful for (device) information which is not time-critical (like received V2X data).
+pub fn set(payload: BleSendable) {
+    critical_section::with(|cs| {
+        let queue_rc = TX_DATA.borrow(cs).borrow();
+        // unwrap is fine b/c we stored something in it before
+        let queue = queue_rc.as_ref().unwrap();
+
+        if queue.push(payload).is_err() {
+            log::error!("BLE TX queue is full");
+        }
+    });
+}
+
 /// Create an advertiser to use to connect to a BLE Central, and wait for it to connect.
 async fn advertise<'values, 'server, C: Controller>(
     peripheral: &mut Peripheral<'values, C, DefaultPacketPool>,
@@ -295,6 +410,12 @@ async fn publishing_task<P: PacketPool>(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) => {
@@ -396,6 +517,33 @@ async fn publishing_task<P: PacketPool>(server: &Server<'_>, conn: &GattConnecti
     }
 }
 
+async fn data_update_task(server: &Server<'_>) {
+    loop {
+        embassy_time::Timer::after(embassy_time::Duration::from_millis(10)).await;
+
+        let mut new_data = None;
+        critical_section::with(|cs| {
+            let queue_rc = TX_DATA.borrow(cs).borrow();
+            // unwrap is fine b/c we stored something in it before
+            let queue = queue_rc.as_ref().unwrap();
+
+            if let Some(data) = queue.pop() {
+                new_data = Some(data);
+            }
+        });
+
+        #[allow(clippy::single_match)]
+        match new_data {
+            Some(BleSendable::BatteryLevel(value)) => {
+                if let Err(err) = server.battery_service.level.set(server, &value) {
+                    warn!("BLE: failed setting new battery level: {err:?}");
+                }
+            }
+            _ => {}
+        }
+    }
+}
+
 async fn send_raw_notify<P: PacketPool>(
     server: &Server<'_>,
     conn: &GattConnection<'_, '_, P>,

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)
@@ -228,7 +234,15 @@ async fn main(spawner: Spawner) -> ! {
             serial[2]
         );
         let short_name = alloc::format!("V2X {:02x}{:02x}", serial[1], serial[2]); // max 8 chars!
-        spawner.spawn(ble::run(controller, name, short_name).expect("Failed to spawn BLE task"));
+
+        let fw_revision = env!("GIT_REVISION");
+        let sw_revision = util::get_feature_list().join(",");
+
+        let device_info = ble::DeviceInfo::new(&mac.to_string(), &fw_revision, &sw_revision);
+
+        spawner.spawn(
+            ble::run(controller, name, short_name, device_info).expect("Failed to spawn BLE task"),
+        );
     }
 
     // Setup GNSS:
@@ -273,6 +287,13 @@ async fn main(spawner: Spawner) -> ! {
     #[cfg(feature = "_gnss")]
     info!("GNSS task started, waiting for GNSS fix...");
 
+    // Setup main loop
+    let (bat_percent, bat_millivolt) = battery.read();
+    log::info!("Battery: {bat_millivolt} mV, {bat_percent} %");
+    #[cfg(feature = "ble")]
+    ble::set(ble::BleSendable::BatteryLevel(bat_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]`
@@ -615,6 +636,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, bat_millivolt) = battery.read();
+            log::info!("Battery: {bat_millivolt} mV, {bat_percent} %");
+
+            #[cfg(feature = "ble")]
+            ble::set(ble::BleSendable::BatteryLevel(bat_percent));
+
+            last_bat_update = cpu_now;
+        }
     }
 }
 

src/util.rs

@@ -31,3 +31,41 @@ pub(crate) fn get_tx_feature_list() -> Vec<&'static str> {
 
     features
 }
+
+#[allow(unused)]
+pub(crate) fn get_feature_list() -> Vec<&'static str> {
+    let mut features = alloc::vec![];
+
+    if cfg!(feature = "gnss_ubx") {
+        features.push("gnss_ubx");
+    }
+    if cfg!(feature = "gnss_l67k") {
+        features.push("gnss_l67k");
+    }
+    if cfg!(feature = "cam_tx") {
+        features.push("cam_tx");
+    }
+    if cfg!(feature = "cam") {
+        features.push("cam");
+    }
+    if cfg!(feature = "denm") {
+        features.push("denm");
+    }
+    if cfg!(feature = "spat") {
+        features.push("spat");
+    }
+    if cfg!(feature = "spat_debug") {
+        features.push("spat_debug");
+    }
+    if cfg!(feature = "screen") {
+        features.push("screen");
+    }
+    if cfg!(feature = "uart") {
+        features.push("uart");
+    }
+    if cfg!(feature = "ble") {
+        features.push("ble");
+    }
+
+    features
+}

src/xiao.rs

@@ -0,0 +1,94 @@
+//! 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<IoAdc, AdcPeripheral<'a>, adc::AdcCalCurve<AdcPeripheral<'a>>>,
+    adc: adc::Adc<'a, AdcPeripheral<'a>, esp_hal::Blocking>,
+}
+
+impl<'a, IoAdc> Battery<'a, IoAdc>
+where
+    IoAdc: adc::AdcChannel + gpio::AnalogPin,
+{
+    pub fn new<IoEnable: gpio::OutputPin>(
+        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
+    #[allow(unused)]
+    pub fn read_percent(&mut self) -> u8 {
+        Self::millivolts_to_percent(self.read_millivolts())
+    }
+
+    /// Returns estimated battery charge state in percent and millivolts
+    pub fn read(&mut self) -> (u8, u16) {
+        let millivolt = self.read_millivolts();
+        let percent = Self::millivolts_to_percent(millivolt);
+
+        (percent, millivolt)
+    }
+
+    /// 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
+        }
+    }
+}