nordic-dev.net/embassy
2
0
Fork 0
mirror of https://github.com/embassy-rs/embassy.git synced 2024-11-26 00:32:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

stm32 CORDIC: ZeroOverhead q1.31 mode

Browse Source
This commit is contained in:
eZio Pan 2024-03-15 19:34:55 +08:00
parent b595d94244
commit a1ca9088b4
2 changed files with 241 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
embassy-stm32/src/cordic/enums.rs
View File

@ -16,14 +16,15 @@ pub enum Function {
/// CORDIC precision
#[allow(missing_docs)]
#[derive(Clone, Copy)]
#[derive(Clone, Copy, Default)]
pub enum Precision {
Iters4 = 1,
Iters8,
Iters12,
Iters16,
Iters20,
Iters24,
#[default]
Iters24, // this value is recomended by Reference Manual
Iters28,
Iters32,
Iters36,
@ -38,7 +39,7 @@ pub enum Precision {
/// CORDIC scale
#[allow(non_camel_case_types)]
#[allow(missing_docs)]
#[derive(Clone, Copy, Default)]
#[derive(Clone, Copy, Default, PartialEq)]
pub enum Scale {
#[default]
A1_R1 = 0,

290
embassy-stm32/src/cordic/mod.rs
View File

@ -10,6 +10,10 @@ pub mod utils;
pub(crate) mod sealed;
// length of pre-allocated [u32] memory for CORDIC input,
// length should be multiple of 2
const INPUT_BUF_LEN: usize = 8;
/// Low-level CORDIC access.
#[cfg(feature = "unstable-pac")]
pub mod low_level {
@ -20,7 +24,7 @@ pub mod low_level {
pub struct Cordic<'d, T: Instance> {
cordic: PeripheralRef<'d, T>,
config: Config,
//state: State,
state: State,
}
/// CORDIC instance trait
@ -28,27 +32,33 @@ pub trait Instance: sealed::Instance + Peripheral<P = Self> + crate::rcc::RccPer
/// CORDIC configuration
pub struct Config {
mode: Mode,
function: Function,
precision: Precision,
scale: Scale,
mode: Mode,
first_result: bool,
}
// CORDIC running state
//struct State {
// input_buf: [u32; 8],
// buf_len: usize,
//}
struct State {
input_buf: [u32; INPUT_BUF_LEN],
buf_index: usize,
}
impl Config {
/// Create a config for Cordic driver
pub fn new(function: Function, precision: Precision, scale: Option<Scale>, mode: Mode, first_result: bool) -> Self {
pub fn new(
mode: Mode,
function: Function,
precision: Option<Precision>,
scale: Option<Scale>,
first_result: bool,
) -> Self {
Self {
function,
precision,
scale: scale.unwrap_or_default(),
mode,
function,
precision: precision.unwrap_or_default(),
scale: scale.unwrap_or_default(),
first_result,
}
}
@ -66,6 +76,7 @@ impl Config {
}
}
// common method
impl<'d, T: Instance> Cordic<'d, T> {
/// Create a Cordic driver instance
///
@ -84,10 +95,10 @@ impl<'d, T: Instance> Cordic<'d, T> {
let mut instance = Self {
cordic,
config,
// state: State {
// input_buf: [0u32; 8],
// buf_len: 0,
// },
state: State {
input_buf: [0u32; 8],
buf_index: 0,
},
};
instance.reconfigure();
@ -128,6 +139,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
peri.set_func(config.function);
peri.set_precision(config.precision);
peri.set_scale(config.scale);
if config.first_result {
peri.set_result_count(Count::One)
} else {
@ -145,44 +157,8 @@ impl<'d, T: Instance> Cordic<'d, T> {
}
}
//self.state.input_buf.fill(0u32);
}
/// Run a CORDIC calculation
pub fn calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
match self.config.mode {
Mode::ZeroOverhead => {
if arg2s.is_none() {
self.cordic.set_argument_count(Count::One);
self.cordic.set_result_count(if self.config.first_result {
if output.len() < arg1s.len() {
panic!("Output buf length is not long enough")
}
Count::One
} else {
if output.len() < 2 * arg1s.len() {
panic!("Output buf length is not long enough")
}
Count::Two
});
let mut cnt = 0;
for &arg in arg1s.iter() {
self.cordic.write_argument(utils::f64_to_q1_31(arg));
output[cnt] = utils::q1_31_to_f64(self.cordic.read_result());
cnt += 1;
}
cnt
} else {
todo!()
}
}
Mode::Interrupt => todo!(),
Mode::Dma => todo!(),
}
self.state.input_buf.fill(0u32);
self.state.buf_index = 0;
}
}
@ -192,8 +168,216 @@ impl<'d, T: Instance> Drop for Cordic<'d, T> {
}
}
// q1.31 related
impl<'d, T: Instance> Cordic<'d, T> {
/// Run a CORDIC calculation
pub fn calc_32bit(&mut self, arg1s: &[f64], arg2s: Option<&[f64]>, output: &mut [f64]) -> usize {
let peri = &self.cordic;
let config = &self.config;
assert!(
match config.first_result {
true => output.len() >= arg1s.len(),
false => output.len() >= 2 * arg1s.len(),
},
"Output buf length is not long enough"
);
self.check_input_f64(arg1s, arg2s);
peri.set_result_count(if config.first_result { Count::One } else { Count::Two });
peri.set_data_width(Width::Bits32, Width::Bits32);
let state = &mut self.state;
let mut output_count = 0;
let mut consumed_input_len = 0;
match config.mode {
Mode::ZeroOverhead => {
// put double input into cordic
if arg2s.is_some() && !arg2s.unwrap().is_empty() {
let arg2s = arg2s.unwrap();
peri.set_argument_count(Count::Two);
let double_value = arg1s.iter().zip(arg2s);
consumed_input_len = double_value.len();
for (arg1, arg2) in double_value {
// if input_buf is full, send values to cordic
if state.buf_index == INPUT_BUF_LEN - 1 {
for arg in state.input_buf.chunks(2) {
peri.write_argument(arg[0]);
peri.write_argument(arg[1]);
output[output_count] = utils::q1_31_to_f64(peri.read_result());
output_count += 1;
if !config.first_result {
output[output_count] = utils::q1_31_to_f64(peri.read_result());
output_count += 1;
}
}
state.buf_index = 0;
}
for &&arg in [arg1, arg2].iter() {
state.input_buf[state.buf_index] = utils::f64_to_q1_31(arg);
state.buf_index += 1;
}
}
// put left paired args into cordic
if state.buf_index > 0 {
for arg in state.input_buf[..state.buf_index].chunks(2) {
peri.write_argument(arg[0]);
peri.write_argument(arg[1]);
output[output_count] = utils::q1_31_to_f64(peri.read_result());
output_count += 1;
if !config.first_result {
output[output_count] = utils::q1_31_to_f64(peri.read_result());
output_count += 1;
}
}
state.buf_index = 0;
}
}
// put single input into cordic
let input_left = &arg1s[consumed_input_len..];
if !input_left.is_empty() {
peri.set_argument_count(Count::One);
for &arg in input_left.iter() {
peri.write_argument(utils::f64_to_q1_31(arg));
output[output_count] = utils::q1_31_to_f64(peri.read_result());
output_count += 1;
if !config.first_result {
output[output_count] = utils::q1_31_to_f64(peri.read_result());
output_count += 1;
}
}
}
output_count
}
Mode::Interrupt => todo!(),
Mode::Dma => todo!(),
}
}
fn check_input_f64(&self, arg1s: &[f64], arg2s: Option<&[f64]>) {
let config = &self.config;
use Function::*;
// check SCALE value
match config.function {
Cos | Sin | Phase | Modulus => assert!(Scale::A1_R1 == config.scale, "SCALE should be 0"),
Arctan => assert!(
(0..=7).contains(&(config.scale as u8)),
"SCALE should be: 0 <= SCALE <= 7"
),
Cosh | Sinh | Arctanh => assert!(Scale::A1o2_R2 == config.scale, "SCALE should be 1"),
Ln => assert!(
(1..=4).contains(&(config.scale as u8)),
"SCALE should be: 1 <= SCALE <= 4"
),
Sqrt => assert!(
(0..=2).contains(&(config.scale as u8)),
"SCALE should be: 0 <= SCALE <= 2"
),
}
// check ARG1 value
match config.function {
Cos | Sin | Phase | Modulus | Arctan => {
assert!(
arg1s.iter().all(|v| (-1.0..=1.0).contains(v)),
"ARG1 should be: -1 <= ARG1 <= 1"
);
}
Cosh | Sinh => assert!(
arg1s.iter().all(|v| (-0.559..=0.559).contains(v)),
"ARG1 should be: -0.559 <= ARG1 <= 0.559"
),
Arctanh => assert!(
arg1s.iter().all(|v| (-0.403..=0.403).contains(v)),
"ARG1 should be: -0.403 <= ARG1 <= 0.403"
),
Ln => {
match config.scale {
Scale::A1o2_R2 => assert!(
arg1s.iter().all(|v| (0.05354..0.5).contains(v)),
"When SCALE set to 1, ARG1 should be: 0.05354 <= ARG1 < 0.5"
),
Scale::A1o4_R4 => assert!(
arg1s.iter().all(|v| (0.25..0.75).contains(v)),
"When SCALE set to 2, ARG1 should be: 0.25 <= ARG1 < 0.75"
),
Scale::A1o8_R8 => assert!(
arg1s.iter().all(|v| (0.375..0.875).contains(v)),
"When SCALE set to 3, ARG1 should be: 0.375 <= ARG1 < 0.875"
),
Scale::A1o16_R16 => assert!(
arg1s.iter().all(|v| (0.4375f64..0.584f64).contains(v)),
"When SCALE set to 4, ARG1 should be: 0.4375 <= ARG1 < 0.584"
),
_ => unreachable!(),
};
}
Function::Sqrt => match config.scale {
Scale::A1_R1 => assert!(
arg1s.iter().all(|v| (0.027..0.75).contains(v)),
"When SCALE set to 0, ARG1 should be: 0.027 <= ARG1 < 0.75"
),
Scale::A1o2_R2 => assert!(
arg1s.iter().all(|v| (0.375..0.875).contains(v)),
"When SCALE set to 1, ARG1 should be: 0.375 <= ARG1 < 0.875"
),
Scale::A1o4_R4 => assert!(
arg1s.iter().all(|v| (0.4375..0.585).contains(v)),
"When SCALE set to 2, ARG1 should be: 0.4375 <= ARG1 < 0.585"
),
_ => unreachable!(),
},
}
// check ARG2 value
if let Some(arg2s) = arg2s {
match config.function {
Cos | Sin => assert!(
arg2s.iter().all(|v| (0.0..=1.0).contains(v)),
"ARG2 should be: 0 <= ARG2 <= 1"
),
Phase | Modulus => assert!(
arg2s.iter().all(|v| (-1.0..=1.0).contains(v)),
"ARG2 should be: -1 <= ARG2 <= 1"
),
_ => (),
}
}
}
}
foreach_interrupt!(
($inst:ident, cordic, CORDIC, GLOBAL, $irq:ident) => {
($inst:ident, cordic, $block:ident, GLOBAL, $irq:ident) => {
impl Instance for peripherals::$inst {
}