Merge pull request #2697 from eZioPan/stm32-cordic

stm32 CORDIC driver

1 files changed, 135 insertions, 0 deletions

diff --git a/tests/stm32/src/bin/cordic.rs b/tests/stm32/src/bin/cordic.rs
new file mode 100644
index 000000000..400e10207
--- /dev/null
+++ b/tests/stm32/src/bin/cordic.rs
@@ -0,0 +1,135 @@
+// required-features: rng, cordic
+
+// Test Cordic driver, with Q1.31 format, Sin function, at 24 iterations (aka PRECISION = 6), using DMA transfer
+
+#![no_std]
+#![no_main]
+
+#[path = "../common.rs"]
+mod common;
+use common::*;
+use embassy_executor::Spawner;
+use embassy_stm32::cordic::utils;
+use embassy_stm32::{bind_interrupts, cordic, peripherals, rng};
+use num_traits::Float;
+use {defmt_rtt as _, panic_probe as _};
+
+bind_interrupts!(struct Irqs {
+   RNG => rng::InterruptHandler<peripherals::RNG>;
+});
+
+/* input value control, can be changed */
+
+const INPUT_U32_COUNT: usize = 9;
+const INPUT_U8_COUNT: usize = 4 * INPUT_U32_COUNT;
+
+// Assume first calculation needs 2 arguments, the reset needs 1 argument.
+// And all calculation generate 2 results.
+const OUTPUT_LENGTH: usize = (INPUT_U32_COUNT - 1) * 2;
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let dp = embassy_stm32::init(config());
+
+    //
+    // use RNG generate random Q1.31 value
+    //
+    // we don't generate floating-point value, since not all binary value are valid floating-point value,
+    // and Q1.31 only accept a fixed range of value.
+
+    let mut rng = rng::Rng::new(dp.RNG, Irqs);
+
+    let mut input_buf_u8 = [0u8; INPUT_U8_COUNT];
+    defmt::unwrap!(rng.async_fill_bytes(&mut input_buf_u8).await);
+
+    // convert every [u8; 4] to a u32, for a Q1.31 value
+    let mut input_q1_31 = unsafe { core::mem::transmute::<[u8; INPUT_U8_COUNT], [u32; INPUT_U32_COUNT]>(input_buf_u8) };
+
+    // ARG2 for Sin function should be inside [0, 1], set MSB to 0 of a Q1.31 value, will make sure it's no less than 0.
+    input_q1_31[1] &= !(1u32 << 31);
+
+    //
+    // CORDIC calculation
+    //
+
+    let mut output_q1_31 = [0u32; OUTPUT_LENGTH];
+
+    // setup Cordic driver
+    let mut cordic = cordic::Cordic::new(
+        dp.CORDIC,
+        defmt::unwrap!(cordic::Config::new(
+            cordic::Function::Sin,
+            Default::default(),
+            Default::default(),
+        )),
+    );
+
+    #[cfg(feature = "stm32g491re")]
+    let (mut write_dma, mut read_dma) = (dp.DMA1_CH4, dp.DMA1_CH5);
+
+    #[cfg(any(feature = "stm32h563zi", feature = "stm32u585ai", feature = "stm32u5a5zj"))]
+    let (mut write_dma, mut read_dma) = (dp.GPDMA1_CH0, dp.GPDMA1_CH1);
+
+    // calculate first result using blocking mode
+    let cnt0 = defmt::unwrap!(cordic.blocking_calc_32bit(&input_q1_31[..2], &mut output_q1_31, false, false));
+
+    // calculate rest results using async mode
+    let cnt1 = defmt::unwrap!(
+        cordic
+            .async_calc_32bit(
+                &mut write_dma,
+                &mut read_dma,
+                &input_q1_31[2..],
+                &mut output_q1_31[cnt0..],
+                true,
+                false,
+            )
+            .await
+    );
+
+    // all output value length should be the same as our output buffer size
+    defmt::assert_eq!(cnt0 + cnt1, output_q1_31.len());
+
+    let mut cordic_result_f64 = [0.0f64; OUTPUT_LENGTH];
+
+    for (f64_val, u32_val) in cordic_result_f64.iter_mut().zip(output_q1_31) {
+        *f64_val = utils::q1_31_to_f64(u32_val);
+    }
+
+    //
+    // software calculation
+    //
+
+    let mut software_result_f64 = [0.0f64; OUTPUT_LENGTH];
+
+    let arg2 = utils::q1_31_to_f64(input_q1_31[1]);
+
+    for (&arg1, res) in input_q1_31
+        .iter()
+        .enumerate()
+        .filter_map(|(idx, val)| if idx != 1 { Some(val) } else { None })
+        .zip(software_result_f64.chunks_mut(2))
+    {
+        let arg1 = utils::q1_31_to_f64(arg1);
+
+        let (raw_res1, raw_res2) = (arg1 * core::f64::consts::PI).sin_cos();
+        (res[0], res[1]) = (raw_res1 * arg2, raw_res2 * arg2);
+    }
+
+    //
+    // check result are the same
+    //
+
+    for (cordic_res, software_res) in cordic_result_f64[..cnt0 + cnt1]
+        .chunks(2)
+        .zip(software_result_f64.chunks(2))
+    {
+        for (cord_res, soft_res) in cordic_res.iter().zip(software_res.iter()) {
+            // 2.0.powi(-19) is the max residual error for Sin function, in q1.31 format, with 24 iterations (aka PRECISION = 6)
+            defmt::assert!((cord_res - soft_res).abs() <= 2.0.powi(-19));
+        }
+    }
+
+    info!("Test OK");
+    cortex_m::asm::bkpt();
+}