Implement distance module (#29)

* Adjust code for distance measurement capability Incorporated a distance detector module in the main codebase. The xe121 example has been expanded and updated to accommodate this module, including necessary modifications for data handling and result processing within the example. All dependencies and relevant functions have likewise been updated to ensure compatibility. * Make RadarState trait public and add to generic constraints The RadarState trait has been made public and added to generic constraints in relevant structures and implementations. Additionally, an unsafe function to get a mutable reference to the sensor was added for completeness. This is part of a broader effort to refine our radar capabilities and ensure robust error handling. * Refactor Radar and Sensor structs to support OutputPin and Delay The Radar, Sensor, and RadarDistanceDetector structs have been extended to incorporate pin outputs and delays, using the embedded_hal and embedded_hal_async libraries. The changes also include edit all related function signatures and calls. This addition provides support for hardware control, enabling and disabling the radar sensor when required. * Update Radar initialization and remove unnecessary pin control * Adjust threshold sensitivity in Radar config * Refactor main function and improve radar calibration * remove unused configs * Refactor DistanceSizes struct to results module * Update build optimization settings and clean up main.rs code * Adjust variable initialization and buffer sizes * Add type alias for result transitions in radar
Ragarnoy · Feb 29, 2024 · b40100c · b40100c
1 parent 41fae95
commit b40100c
Show file tree

Hide file tree

Showing 18 changed files with 989 additions and 117 deletions.
diff --git a/Cargo.toml b/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"

diff --git a/build.rs b/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()

diff --git a/examples/xe121_l433rc/Cargo.toml b/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"
+
diff --git a/examples/xe121_l433rc/src/io.rs b/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(());
+    }
+}
diff --git a/examples/xe121_l433rc/src/main.rs b/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)
+}
diff --git a/examples/xe125/Cargo.lock b/examples/xe125/Cargo.lock
diff --git a/examples/xe125/Cargo.toml b/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"