up

bluetooth_recorder/Cargo.toml

@@ -0,0 +1,43 @@
+[package]
+name = "bluetooth_recorder"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[features]
+nrf52840 = ["embassy-nrf/nrf52840", "nrf-sdc/nrf52840"]
+
+[dependencies]
+embassy-futures.workspace = true
+embassy-executor.workspace = true
+embassy-time.workspace = true
+embassy-nrf.workspace = true
+embassy-embedded-hal.workspace = true
+embassy-sync.workspace = true
+
+futures.workspace = true
+nrf-sdc.workspace = true
+nrf-mpsl.workspace = true
+bt-hci.workspace = true
+trouble-host.workspace = true
+trouble-example-apps.workspace = true
+
+defmt.workspace = true
+defmt-rtt.workspace = true
+
+cortex-m.workspace = true
+cortex-m-rt.workspace = true
+panic-probe.workspace = true
+rand.workspace = true
+static_cell.workspace = true
+rand_core.workspace = true
+rand_chacha.workspace = true
+
+libm.workspace = true
+heapless.workspace = true
+lsm6ds3tr.workspace = true
+
+fixed.workspace = true
+atomic-pool.workspace = true
+# critical-once-cell.workspace = true

bluetooth_recorder/src/main.rs

@@ -0,0 +1,489 @@
+#![no_std]
+#![no_main]
+
+use core::ops::Deref;
+
+use core::fmt::Write;
+use defmt::{info, unwrap, warn};
+use embassy_executor::Spawner;
+use embassy_nrf::gpio::{AnyPin, Level, Output, OutputDrive};
+use embassy_nrf::mode::Async;
+use embassy_nrf::peripherals::{self, RNG};
+use embassy_nrf::pwm::SequenceMode;
+use embassy_nrf::{bind_interrupts, rng, twim, Peri, Peripherals};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::mutex::Mutex;
+use heapless::String;
+use lsm6ds3tr::interface::I2cInterface;
+use lsm6ds3tr::{
+    registers::{GyroSampleRate, GyroScale},
+    AccelSampleRate, AccelScale, AccelSettings, GyroSettings, LsmSettings, LSM6DS3TR,
+};
+use nrf_sdc::mpsl::MultiprotocolServiceLayer;
+use nrf_sdc::{self as sdc, mpsl};
+use static_cell::{ConstStaticCell, StaticCell};
+use trouble_host::prelude::*;
+use {defmt_rtt as _, panic_probe as _};
+
+use embassy_futures::join::join;
+use embassy_futures::select::select;
+use embassy_time::Timer;
+
+bind_interrupts!(struct Irqs {
+    RNG => rng::InterruptHandler<RNG>;
+    EGU0_SWI0 => nrf_sdc::mpsl::LowPrioInterruptHandler;
+    CLOCK_POWER => nrf_sdc::mpsl::ClockInterruptHandler;
+    RADIO => nrf_sdc::mpsl::HighPrioInterruptHandler;
+    TIMER0 => nrf_sdc::mpsl::HighPrioInterruptHandler;
+    RTC0 => nrf_sdc::mpsl::HighPrioInterruptHandler;
+});
+
+#[embassy_executor::task]
+async fn mpsl_task(mpsl: &'static MultiprotocolServiceLayer<'static>) -> ! {
+    mpsl.run().await
+}
+
+/// How many outgoing L2CAP buffers per link
+const L2CAP_TXQ: u8 = 3;
+
+/// How many incoming L2CAP buffers per link
+const L2CAP_RXQ: u8 = 3;
+
+/// Max number of connections
+const CONNECTIONS_MAX: usize = 1;
+
+/// Max number of L2CAP channels.
+const L2CAP_CHANNELS_MAX: usize = 2; // Signal + att
+
+// GATT Server definition
+#[gatt_server]
+struct Server {
+    fake_service: FakeService,
+    led_service: LedService,
+    gyro_service: GyroService,
+}
+
+/// fakr service
+#[gatt_service(uuid = "18B10000-E8F2-537E-4F6C-D104768A1210")]
+struct FakeService {
+    /// Fake Life
+    #[descriptor(uuid = descriptors::VALID_RANGE, read, value = [0, 100])]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "hello", read, value = "Fake Life")]
+    #[characteristic(
+        uuid = "407813df-5dd4-1f87-ec11-cdb001100000",
+        read,
+        notify,
+        value = 10
+    )]
+    level: u8,
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "boolean", read, value = "Bollean")]
+    #[characteristic(uuid = "408813df-5dd4-1f87-ec11-cdb001100000", write, read, notify)]
+    status: bool,
+}
+
+#[gatt_service(uuid = "17B10000-E8F2-537E-4F6C-D104768A1214")]
+struct LedService {
+    #[descriptor(
+        uuid = descriptors::MEASUREMENT_DESCRIPTION,
+        name = "led",
+        read,
+        value = "Led Toggle"
+    )]
+    #[characteristic(
+        uuid = "17B10000-E8F2-537E-4F6C-D104768A1215",
+        write,
+        read,
+        notify,
+        value = false
+    )]
+    on_off: bool,
+}
+
+#[gatt_service(uuid = "19B10000-E8F2-537E-4F6C-D104768A1214")]
+struct GyroService {
+    #[descriptor(
+        uuid = descriptors::MEASUREMENT_DESCRIPTION,
+        name = "gyro_toggle",
+        read,
+        value = "Gyro Toggle"
+    )]
+    #[characteristic(
+        uuid = "19B10000-E8F2-537E-4F6C-D104768A1215",
+        write,
+        read,
+        notify,
+        value = false
+    )]
+    on_off: bool, // This characteristic controls whether IMU data is sent
+    #[descriptor(
+        uuid = descriptors::MEASUREMENT_DESCRIPTION,
+        name = "imu_data",
+        read,
+        value = "Combined Gyro and Accel Data"
+    )]
+    #[characteristic(uuid = "19B10000-E8F2-537E-4F6C-D104768A1216", read, notify, value = [0;32])]
+    imu_data: [u8; 32], // Gyro (X, Y, Z), Accel (X, Y, Z) as 6 f32s = 24 bytes + Timestamp (u64) = 8 bytes = 32 bytes
+}
+fn build_sdc<'d, const N: usize>(
+    p: nrf_sdc::Peripherals<'d>,
+    rng: &'d mut rng::Rng<RNG, Async>,
+    mpsl: &'d MultiprotocolServiceLayer,
+    mem: &'d mut sdc::Mem<N>,
+) -> Result<nrf_sdc::SoftdeviceController<'d>, nrf_sdc::Error> {
+    sdc::Builder::new()?
+        .support_adv()?
+        .support_peripheral()?
+        .peripheral_count(1)?
+        .buffer_cfg(
+            DefaultPacketPool::MTU as u16,
+            DefaultPacketPool::MTU as u16,
+            L2CAP_TXQ,
+            L2CAP_RXQ,
+        )?
+        .build(p, rng, mpsl, mem)
+}
+
+bind_interrupts!(struct IrqsTest {
+    TWISPI0 => twim::InterruptHandler<peripherals::TWISPI0>;
+});
+
+type Imu<'a> = LSM6DS3TR<I2cInterface<twim::Twim<'a, peripherals::TWISPI0>>>;
+static IMU: StaticCell<Mutex<CriticalSectionRawMutex, Imu<'static>>> = StaticCell::new();
+
+#[embassy_executor::main]
+async fn main(spawner: Spawner) {
+    let p = embassy_nrf::init(Default::default());
+
+    let mut pin = Output::new(p.P1_08, Level::High, OutputDrive::HighDrive);
+    pin.set_high();
+    Timer::after_millis(100).await;
+
+    let sda_pin = p.P0_07;
+    let scl_pin = p.P0_27;
+
+    let mut imu_config = twim::Config::default();
+    // This limits the ADXL355 ODR to 200Hz max.
+    imu_config.frequency = twim::Frequency::K400;
+    // Internal pullups for SCL and SDA must be enabled.
+    imu_config.scl_pullup = true;
+    imu_config.sda_pullup = true;
+
+    static RAM_BUFFER: ConstStaticCell<[u8; 16]> = ConstStaticCell::new([0; 16]);
+
+    let i2c = twim::Twim::new(
+        p.TWISPI0,
+        IrqsTest,
+        sda_pin,
+        scl_pin,
+        imu_config,
+        RAM_BUFFER.take(),
+    );
+    let settings = LsmSettings::basic()
+        .with_gyro(
+            GyroSettings::new()
+                .with_sample_rate(GyroSampleRate::_104Hz)
+                .with_scale(GyroScale::_2000DPS),
+        )
+        .with_accel(
+            AccelSettings::new()
+                .with_sample_rate(AccelSampleRate::_104Hz)
+                .with_scale(AccelScale::_4G),
+        );
+
+    let mut imu_driver = LSM6DS3TR::new(I2cInterface::new(i2c)).with_settings(settings);
+    imu_driver
+        .init()
+        .expect("LSM6DS3TR-C initialization failure!");
+
+    let imu = IMU.init(Mutex::new(imu_driver));
+
+    let mpsl_p =
+        mpsl::Peripherals::new(p.RTC0, p.TIMER0, p.TEMP, p.PPI_CH19, p.PPI_CH30, p.PPI_CH31);
+    let lfclk_cfg = mpsl::raw::mpsl_clock_lfclk_cfg_t {
+        source: mpsl::raw::MPSL_CLOCK_LF_SRC_RC as u8,
+        rc_ctiv: mpsl::raw::MPSL_RECOMMENDED_RC_CTIV as u8,
+        rc_temp_ctiv: mpsl::raw::MPSL_RECOMMENDED_RC_TEMP_CTIV as u8,
+        accuracy_ppm: mpsl::raw::MPSL_DEFAULT_CLOCK_ACCURACY_PPM as u16,
+        skip_wait_lfclk_started: mpsl::raw::MPSL_DEFAULT_SKIP_WAIT_LFCLK_STARTED != 0,
+    };
+    static MPSL: StaticCell<MultiprotocolServiceLayer> = StaticCell::new();
+    let mpsl = MPSL.init(unwrap!(mpsl::MultiprotocolServiceLayer::new(
+        mpsl_p, Irqs, lfclk_cfg
+    )));
+    spawner.must_spawn(mpsl_task(&*mpsl));
+
+    let sdc_p = sdc::Peripherals::new(
+        p.PPI_CH17, p.PPI_CH18, p.PPI_CH20, p.PPI_CH21, p.PPI_CH22, p.PPI_CH23, p.PPI_CH24,
+        p.PPI_CH25, p.PPI_CH26, p.PPI_CH27, p.PPI_CH28, p.PPI_CH29,
+    );
+
+    let mut rng = rng::Rng::new(p.RNG, Irqs);
+
+    let mut sdc_mem = sdc::Mem::<4720>::new();
+    let sdc = unwrap!(build_sdc(sdc_p, &mut rng, mpsl, &mut sdc_mem));
+
+    let led_pin = p.P0_06.into();
+
+    // peripheral::run(sdc).await;
+    run(sdc, led_pin, imu).await;
+}
+
+/// Run the BLE stack.
+pub async fn run<C>(
+    controller: C,
+    led_pin: Peri<'static, AnyPin>,
+    imu: &'static Mutex<CriticalSectionRawMutex, Imu<'static>>,
+) where
+    C: Controller,
+{
+    let mut led = Output::new(led_pin, Level::Low, OutputDrive::Standard);
+
+    // Using a fixed "random" address can be useful for testing. In real scenarios, one would
+    // use e.g. the MAC 6 byte array as the address (how to get that varies by the platform).
+    let address: Address = Address::random([0xff, 0x8f, 0x1a, 0x05, 0xe4, 0xff]);
+    // info!("Our address = {:?}", address);
+
+    let mut resources: HostResources<DefaultPacketPool, CONNECTIONS_MAX, L2CAP_CHANNELS_MAX> =
+        HostResources::new();
+    let stack = trouble_host::new(controller, &mut resources).set_random_address(address);
+    let Host {
+        mut peripheral,
+        runner,
+        ..
+    } = stack.build();
+
+    // info!("Starting advertising and GATT service");
+    let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig {
+        name: "TrouBLE",
+        appearance: &appearance::power_device::GENERIC_POWER_DEVICE,
+    }))
+    .unwrap();
+
+    let _ = join(ble_task(runner), async {
+        loop {
+            match advertise("Trouble Example", &mut peripheral, &server).await {
+                Ok(conn) => {
+                    // set up tasks when the connection is established to a central, so they don't run when no one is connected.
+                    let a = gatt_events_task(&server, &conn);
+                    let b = custom_task(&server, &conn, &stack, &mut led, imu);
+                    // run until any task ends (usually because the connection has been closed),
+                    // then return to advertising state.
+                    select(a, b).await;
+                    // a.await;
+
+                    led.set_low();
+                }
+                Err(e) => {
+                    panic!("[adv] error: {:?}", e);
+                }
+            }
+        }
+    })
+    .await;
+}
+
+/// This is a background task that is required to run forever alongside any other BLE tasks.
+///
+/// ## Alternative
+///
+/// If you didn't require this to be generic for your application, you could statically spawn this with i.e.
+///
+/// ```rust,ignore
+///
+/// #[embassy_executor::task]
+/// async fn ble_task(mut runner: Runner<'static, SoftdeviceController<'static>>) {
+///     runner.run().await;
+/// }
+///
+/// spawner.must_spawn(ble_task(runner));
+/// ```
+async fn ble_task<C: Controller, P: PacketPool>(mut runner: Runner<'_, C, P>) {
+    loop {
+        if let Err(e) = runner.run().await {
+            panic!("[ble_task] error: {:?}", e);
+        }
+    }
+}
+
+/// Stream Events until the connection closes.
+///
+/// This function will handle the GATT events and process them.
+/// This is how we interact with read and write requests.
+async fn gatt_events_task<P: PacketPool>(
+    server: &Server<'_>,
+    conn: &GattConnection<'_, '_, P>,
+) -> Result<(), Error> {
+    let fake_level = server.fake_service.level;
+    let led_level = server.led_service.handle;
+    let reason = loop {
+        match conn.next().await {
+            GattConnectionEvent::Disconnected { reason } => break reason,
+            GattConnectionEvent::Gatt { event } => {
+                match &event {
+                    GattEvent::Read(event) => {
+                        // if event.handle() == fake_level.handle {
+                        //     let value = server.get(&fake_level);
+                        //     // info!("[gatt] Read Event to Level Characteristic: {:?}", value);
+                        // }
+                        if event.handle() == led_level {
+                            let value = server.get(&server.led_service.on_off);
+                            // info!("[gatt] Read Event to Level Characteristic: {:?}", value);
+                        }
+                    }
+                    GattEvent::Write(event) => {
+                        // if event.handle() == fake_level.handle {
+                        //     // info!(
+                        //     //     "[gatt] Write Event to Level Characteristic: {:?}",
+                        //     //     event.data()
+                        //     // );
+                        // }
+                        if event.handle() == led_level {
+                            // info!(
+                            //     "[gatt] Write Event to Level Characteristic: {:?}",
+                            //     event.data()
+                            // );
+                        }
+                    }
+                    _ => {}
+                };
+                // This step is also performed at drop(), but writing it explicitly is necessary
+                // in order to ensure reply is sent.
+                match event.accept() {
+                    Ok(reply) => reply.send().await,
+                    Err(e) => panic!("[gatt] error sending response"),
+                };
+            }
+            _ => {} // ignore other Gatt Connection Events
+        }
+    };
+    // info!("[gatt] disconnected: {:?}", reason);
+    Ok(())
+}
+
+/// Create an advertiser to use to connect to a BLE Central, and wait for it to connect.
+async fn advertise<'values, 'server, C: Controller>(
+    name: &'values str,
+    peripheral: &mut Peripheral<'values, C, DefaultPacketPool>,
+    server: &'server Server<'values>,
+) -> Result<GattConnection<'values, 'server, DefaultPacketPool>, BleHostError<C::Error>> {
+    let mut advertiser_data = [0; 31];
+    let len = AdStructure::encode_slice(
+        &[
+            AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
+            AdStructure::ServiceUuids16(&[[0x0f, 0x18]]),
+            AdStructure::CompleteLocalName(name.as_bytes()),
+        ],
+        &mut advertiser_data[..],
+    )?;
+    let advertiser = peripheral
+        .advertise(
+            &Default::default(),
+            Advertisement::ConnectableScannableUndirected {
+                adv_data: &advertiser_data[..len],
+                scan_data: &[],
+            },
+        )
+        .await?;
+    // info!("[adv] advertising");
+    let conn = advertiser.accept().await?.with_attribute_server(server)?;
+    // info!("[adv] connection established");
+    Ok(conn)
+}
+
+/// Example task to use the BLE notifier interface.
+/// This task will notify the connected central of a counter value every 2 seconds.
+/// It will also read the RSSI value every 2 seconds.
+/// and will stop when the connection is closed by the central or an error occurs.
+async fn custom_task<C: Controller, P: PacketPool>(
+    server: &Server<'_>,
+    conn: &GattConnection<'_, '_, P>,
+    stack: &Stack<'_, C, P>,
+    led: &mut Output<'static>,
+    imu: &'static Mutex<CriticalSectionRawMutex, Imu<'static>>,
+) {
+    let mut tick: u8 = 0;
+    let fake_level = server.fake_service.level;
+    let led_on_off = server.led_service.on_off;
+    let gyro_service_on_off = server.gyro_service.on_off; // Get gyro service toggle
+    let imu_data_char = &server.gyro_service.imu_data; // Combined IMU data characteristic
+
+    loop {
+        // Only read and send IMU data if the gyro_service_on_off characteristic is true
+        if gyro_service_on_off.get(server).unwrap_or(false) {
+            let mut imu_guard = imu.lock().await;
+
+            if let (Ok(xyz_a), Ok(xyz_g)) = (imu_guard.read_accel_raw(), imu_guard.read_gyro()) {
+                info!("Accel: x:{} y:{} z:{}", xyz_a.x, xyz_a.y, xyz_a.z);
+                info!("Gyro: x:{} y:{} z:{}", xyz_g.x, xyz_g.y, xyz_g.z);
+
+                // Capture timestamp immediately after reading IMU data for better accuracy
+                let current_time_us = embassy_time::Instant::now().as_micros();
+
+                // Convert i32 gyro values to f32 and then to their byte representation
+                let gx_f32 = xyz_g.x as f32;
+                let gy_f32 = xyz_g.y as f32;
+                let gz_f32 = xyz_g.z as f32;
+
+                // Convert i32 accel values to f32 and then to their byte representation
+                let ax_f32 = xyz_a.x as f32;
+                let ay_f32 = xyz_a.y as f32;
+                let az_f32 = xyz_a.z as f32;
+
+                let gx_bytes = gx_f32.to_le_bytes();
+                let gy_bytes = gy_f32.to_le_bytes();
+                let gz_bytes = gz_f32.to_le_bytes();
+                let ax_bytes = ax_f32.to_le_bytes();
+                let ay_bytes = ay_f32.to_le_bytes();
+                let az_bytes = az_f32.to_le_bytes();
+
+                // Combine gyro, accel, and timestamp byte arrays into a single [u8; 32] array
+                // Order: Gx, Gy, Gz, Ax, Ay, Az (each 4 bytes) then Timestamp (8 bytes)
+                let mut combined_imu_data = [0u8; 24];
+                combined_imu_data[0..4].copy_from_slice(&gx_bytes);
+                combined_imu_data[4..8].copy_from_slice(&gy_bytes);
+                combined_imu_data[8..12].copy_from_slice(&gz_bytes);
+                combined_imu_data[12..16].copy_from_slice(&ax_bytes);
+                combined_imu_data[16..20].copy_from_slice(&ay_bytes);
+                combined_imu_data[20..24].copy_from_slice(&az_bytes);
+                let timestamp_bytes = current_time_us.to_le_bytes();
+
+                let mut final_imu_packet = [0u8; 32];
+                final_imu_packet[0..24].copy_from_slice(&combined_imu_data);
+                final_imu_packet[24..32].copy_from_slice(&timestamp_bytes);
+
+                // Update combined IMU characteristic and notify
+                imu_data_char.set(server, &final_imu_packet);
+                if imu_data_char.notify(conn, &final_imu_packet).await.is_err() {
+                    info!(
+                        "[custom_task] error notifying combined IMU and timestamp characteristic"
+                    );
+                    break;
+                };
+            } else {
+                warn!("Could not read IMU data");
+            }
+        } // End of gyro_service_on_off check
+
+        tick = tick.wrapping_add(1);
+        // info!("[custom_task] notifying connection of tick {}", tick);
+        if fake_level.notify(conn, &tick).await.is_err() {
+            // info!("[custom_task] error notifying connection");
+            break;
+        };
+        // read RSSI (Received Signal Strength Indicator) of the connection.
+        if let Ok(rssi) = conn.raw().rssi(stack).await {
+            // info!("[custom_task] RSSI: {:?}", rssi);
+        } else {
+            // info!("[custom_task] error getting RSSI");
+            break;
+        };
+        if led_on_off.get(server).unwrap() == true {
+            led.set_high();
+        }
+        // Timer::after_millis(100).await;
+        if led_on_off.get(server).unwrap() == true {
+            led.set_low();
+        }
+        Timer::after_millis(100).await
+    }
+}