135 lines
4.1 KiB
Rust
135 lines
4.1 KiB
Rust
// 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();
|
|
}
|