Implement distance module

Cargo.toml

@@ -12,9 +12,7 @@ embedded-hal-async = "1.0.0"
 embassy-sync = { git = "https://github.com/embassy-rs/embassy", features = [ "defmt" ] }
 
 tinyrlibc = { version = "*", git = "https://github.com/rust-embedded-community/tinyrlibc", features = ["snprintf", "alloc"], default-features = false }
-talc = { version = "4.2", default-features = false, features = ["lock_api"] }
-num = { version = "0.4", default-features = false }
-spin = "0.9.8"
+num = { version = "0.4", default-features = false, features = ["libm"]}
 
 [build-dependencies]
 bindgen = "0.69"

build.rs

@@ -21,6 +21,7 @@ fn main() {
 
     println!("cargo:rustc-link-search={}", lib.display());
     println!("cargo:rustc-link-lib=static=acconeer_a121");
+    println!("cargo:rustc-link-lib=static=acc_detector_distance_a121");
     println!(
         "cargo:rerun-if-changed={}",
         xmpath.join("include").display()

examples/xe121_l433rc/Cargo.toml

@@ -15,14 +15,17 @@ defmt = "0.3.5"
 defmt-rtt = "0.4.0"
 
 embedded-hal = "1.0.0"
-embassy-executor = { version = "0.5.0", features = [ "task-arena-size-8192", "arch-cortex-m", "executor-thread", "defmt", "integrated-timers" ] }
+embassy-executor = { version = "0.5.0", features = [ "task-arena-size-20480", "arch-cortex-m", "executor-thread", "defmt", "integrated-timers" ] }
 embassy-time = { version = "0.3.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768" ]}
 embassy-stm32 = { version = "0.1.0", features = [ "defmt", "unstable-pac", "stm32l433rc", "memory-x", "time-driver-any", "exti", "chrono"] }
 embassy-sync = "0.5"
 embassy-embedded-hal = { version = "0.1", features = ["defmt"] }
 embedded-hal-bus = { version = "0.1.0", features = ["defmt-03"] }
 
 libm = { version = "0.2.8", default-features = false }
+talc = { version = "4.2", default-features = false, features = ["lock_api"] }
+spin = "0.9.8"
+num = { version = "0.4", default-features = false, features = ["libm"]}
 
 static_cell = "2.0.0"
 
@@ -34,3 +37,6 @@ codegen-units = 1
 [profile.release]
 debug = 2
 codegen-units = 1
+lto = true
+opt-level = "s"
+

examples/xe121_l433rc/src/io.rs

@@ -0,0 +1,15 @@
+use embassy_stm32::exti::ExtiInput;
+use embassy_stm32::peripherals::PC13;
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::signal::Signal;
+
+pub static LED_SIGNAL: Signal<CriticalSectionRawMutex, ()> = Signal::new();
+
+#[embassy_executor::task]
+pub async fn button_task(mut button: ExtiInput<'static, PC13>) {
+    // Await the button press
+    loop {
+        button.wait_for_high().await;
+        LED_SIGNAL.signal(());
+    }
+}

examples/xe121_l433rc/src/main.rs

@@ -3,7 +3,7 @@
 
 use core::cell::RefCell;
 
-use a121_rs::config::profile::RadarProfile::AccProfile5;
+use a121_rs::detector::distance::RadarDistanceDetector;
 use a121_rs::radar;
 use a121_rs::radar::Radar;
 use defmt::{info, warn};
@@ -19,23 +19,36 @@ use embassy_stm32::spi::{Config, Spi};
 use embassy_stm32::time::Hertz;
 use embassy_time::{Delay, Duration, Timer};
 use embedded_hal_bus::spi::ExclusiveDevice;
+use num::complex::Complex32;
 use radar::rss_version;
+use talc::{ClaimOnOom, Span, Talc, Talck};
 use {defmt_rtt as _, panic_probe as _};
 
 use crate::adapter::SpiAdapter;
 
 mod adapter;
+pub mod io;
+
+static mut ARENA: [u8; 10000] = [0; 10000];
+
+#[global_allocator]
+static ALLOCATOR: Talck<spin::Mutex<()>, ClaimOnOom> = Talc::new(unsafe {
+    // if we're in a hosted environment, the Rust runtime may allocate before
+    // main() is called, so we need to initialize the arena automatically
+    ClaimOnOom::new(Span::from_const_array(core::ptr::addr_of!(ARENA)))
+})
+.lock();
 
 type SpiDeviceMutex =
     ExclusiveDevice<Spi<'static, SPI2, NoDma, NoDma>, Output<'static, PA11>, Delay>;
 static mut SPI_DEVICE: Option<RefCell<SpiAdapter<SpiDeviceMutex>>> = None;
 
 #[embassy_executor::main]
-async fn main(_spawner: Spawner) {
+async fn main(spawner: Spawner) {
     info!("Starting");
     let p = embassy_stm32::init(xm125_clock_config());
 
-    let mut enable = Output::new(p.PC2, Level::Low, Speed::VeryHigh); // ENABLE on PB12
+    let enable = Output::new(p.PC2, Level::Low, Speed::VeryHigh);
     let cs_pin = Output::new(p.PA11, Level::High, Speed::VeryHigh);
     let _sel0 = Output::new(p.PC3, Level::Low, Speed::VeryHigh);
     let _sel1 = Output::new(p.PA1, Level::Low, Speed::VeryHigh);
@@ -44,6 +57,9 @@ async fn main(_spawner: Spawner) {
     let interrupt = ExtiInput::new(input, p.EXTI6);
     info!("GPIO initialized");
 
+    let button = Input::new(p.PC13, Pull::Down);
+    let button = ExtiInput::new(button, p.EXTI13);
+
     let spi = Spi::new(
         p.SPI2,
         p.PB13, // SCK
@@ -59,37 +75,71 @@ async fn main(_spawner: Spawner) {
     unsafe { SPI_DEVICE = Some(RefCell::new(SpiAdapter::new(exclusive_device))) };
     let spi_mut_ref = unsafe { SPI_DEVICE.as_mut().unwrap() };
 
-    enable.set_high();
-    Timer::after(Duration::from_millis(2)).await;
-
     info!("RSS Version: {}", rss_version());
 
-    let mut radar = Radar::new(1, spi_mut_ref.get_mut(), interrupt);
-    radar.config.set_profile(AccProfile5);
+    let mut radar = Radar::new(1, spi_mut_ref.get_mut(), interrupt, enable, Delay).await;
     info!("Radar enabled");
     let mut buffer = [0u8; 2560];
-    let mut radar = loop {
+    let mut calibration = loop {
         buffer.fill(0);
-        if let Ok(mut calibration) = radar.calibrate().await {
+        if let Ok(calibration) = radar.calibrate().await {
             if let Ok(()) = calibration.validate_calibration() {
                 info!("Calibration is valid");
-                break radar.prepare_sensor(&mut calibration).unwrap();
+                break calibration;
             } else {
                 warn!("Calibration is invalid");
                 warn!("Calibration result: {:?}", calibration);
-                enable.set_low();
             }
         } else {
             warn!("Calibration failed");
         }
         Timer::after(Duration::from_millis(1)).await;
     };
     info!("Calibration complete!");
+    let mut radar = radar.prepare_sensor(&mut calibration).unwrap();
+    let mut distance = RadarDistanceDetector::new(&mut radar);
+    let mut buffer = [0u8; 2560 * 3];
+    let mut static_call_result = [0u8; 2560];
+    let mut dynamic_call_result = distance
+        .calibrate_detector(&calibration, &mut buffer, &mut static_call_result)
+        .await
+        .unwrap();
+
+    spawner.spawn(io::button_task(button)).unwrap();
 
     loop {
         Timer::after(Duration::from_millis(200)).await;
-        let data = radar.measure().await.unwrap();
-        info!("Data: {:?}", data);
+        'inner: loop {
+            distance
+                .prepare_detector(&calibration, &mut buffer)
+                .unwrap();
+            distance.measure().await.unwrap();
+
+            if let Ok(res) = distance.process_data(
+                &mut buffer,
+                &mut static_call_result,
+                &mut dynamic_call_result,
+            ) {
+                if res.num_distances() > 0 {
+                    info!(
+                        "{} Distances found:\n{:?}",
+                        res.num_distances(),
+                        res.distances()
+                    );
+                }
+                if res.calibration_needed() {
+                    info!("Calibration needed");
+                    break 'inner;
+                }
+            } else {
+                warn!("Failed to process data");
+            }
+        }
+        let calibration = distance.calibrate().await.unwrap();
+        dynamic_call_result = distance
+            .update_calibration(&calibration, &mut buffer)
+            .await
+            .unwrap();
     }
 }
 
@@ -144,3 +194,38 @@ pub extern "C" fn __hardfp_roundf(f: f32) -> f32 {
 pub extern "C" fn __hardfp_sqrtf(f: f32) -> f32 {
     libm::sqrtf(f)
 }
+
+#[no_mangle]
+pub extern "C" fn __hardfp_powf(f: f32, g: f32) -> f32 {
+    libm::powf(f, g)
+}
+
+#[no_mangle]
+pub extern "C" fn __hardfp_cexpf(f: Complex32) -> Complex32 {
+    f.exp()
+}
+
+#[no_mangle]
+pub extern "C" fn __hardfp_cabsf(f: f32) -> f32 {
+    libm::fabsf(f)
+}
+
+#[no_mangle]
+pub extern "C" fn __hardfp_atanf(f: f32) -> f32 {
+    libm::atanf(f)
+}
+
+#[no_mangle]
+pub extern "C" fn __hardfp_floorf(f: f32) -> f32 {
+    libm::floorf(f)
+}
+
+#[no_mangle]
+pub extern "C" fn __hardfp_log10f(f: f32) -> f32 {
+    libm::log10f(f)
+}
+
+#[no_mangle]
+pub extern "C" fn __hardfp_exp2f(f: f32) -> f32 {
+    libm::exp2f(f)
+}

examples/xe125/Cargo.toml

@@ -15,22 +15,26 @@ defmt = "0.3.5"
 defmt-rtt = "0.4.0"
 
 embedded-hal = "1.0.0"
-embassy-executor = { version = "0.5.0", features = [ "task-arena-size-8192", "arch-cortex-m", "executor-thread", "defmt", "integrated-timers" ] }
+embassy-executor = { version = "0.5.0", features = [ "task-arena-size-20480", "arch-cortex-m", "executor-thread", "defmt", "integrated-timers" ] }
 embassy-time = { version = "0.3.0", features = [ "defmt", "defmt-timestamp-uptime", "tick-hz-32_768" ]}
 embassy-stm32 = { version = "0.1.0", features = [ "defmt", "unstable-pac", "stm32l431cb", "memory-x", "time-driver-any", "exti", "chrono"] }
 embassy-sync = "0.5"
 embassy-embedded-hal = { version = "0.1", features = ["defmt"] }
 embedded-hal-bus = { version = "0.1.0", features = ["defmt-03"] }
 
 libm = { version = "0.2.8", default-features = false }
+talc = { version = "4.2", default-features = false, features = ["lock_api"] }
+spin = "0.9.8"
+num = { version = "0.4", default-features = false, features = ["libm"]}
 
 static_cell = "2.0.0"
 
-
 [profile.dev]
 lto = true
 codegen-units = 1
 
 [profile.release]
 debug = 2
+lto = true
 codegen-units = 1
+opt-level = "s"