stm32 CORDIC: bug fix
This commit is contained in:
parent
641da3602e
commit
c42d9f9eaa
3 changed files with 109 additions and 63 deletions
|
@ -9,6 +9,26 @@ pub enum CordicError {
|
||||||
ArgError(ArgError),
|
ArgError(ArgError),
|
||||||
/// Output buffer length error
|
/// Output buffer length error
|
||||||
OutputLengthNotEnough,
|
OutputLengthNotEnough,
|
||||||
|
/// Input value is out of range for Q1.x format
|
||||||
|
NumberOutOfRange(NumberOutOfRange),
|
||||||
|
}
|
||||||
|
|
||||||
|
impl From<ConfigError> for CordicError {
|
||||||
|
fn from(value: ConfigError) -> Self {
|
||||||
|
Self::ConfigError(value)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl From<ArgError> for CordicError {
|
||||||
|
fn from(value: ArgError) -> Self {
|
||||||
|
Self::ArgError(value)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl From<NumberOutOfRange> for CordicError {
|
||||||
|
fn from(value: NumberOutOfRange) -> Self {
|
||||||
|
Self::NumberOutOfRange(value)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[cfg(feature = "defmt")]
|
#[cfg(feature = "defmt")]
|
||||||
|
@ -19,6 +39,7 @@ impl defmt::Format for CordicError {
|
||||||
match self {
|
match self {
|
||||||
ConfigError(e) => defmt::write!(fmt, "{}", e),
|
ConfigError(e) => defmt::write!(fmt, "{}", e),
|
||||||
ArgError(e) => defmt::write!(fmt, "{}", e),
|
ArgError(e) => defmt::write!(fmt, "{}", e),
|
||||||
|
NumberOutOfRange(e) => defmt::write!(fmt, "{}", e),
|
||||||
OutputLengthNotEnough => defmt::write!(fmt, "Output buffer length is not long enough"),
|
OutputLengthNotEnough => defmt::write!(fmt, "Output buffer length is not long enough"),
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -68,28 +89,51 @@ impl defmt::Format for ArgError {
|
||||||
defmt::write!(fmt, " when SCALE is {},", scale);
|
defmt::write!(fmt, " when SCALE is {},", scale);
|
||||||
}
|
}
|
||||||
|
|
||||||
let arg_string = match self.arg_type {
|
defmt::write!(fmt, " {} should be", self.arg_type);
|
||||||
ArgType::Arg1 => "ARG1",
|
|
||||||
ArgType::Arg2 => "ARG2",
|
if self.inclusive_upper_bound {
|
||||||
|
defmt::write!(
|
||||||
|
fmt,
|
||||||
|
" {} <= {} <= {}",
|
||||||
|
self.arg_range[0],
|
||||||
|
self.arg_type,
|
||||||
|
self.arg_range[1]
|
||||||
|
)
|
||||||
|
} else {
|
||||||
|
defmt::write!(
|
||||||
|
fmt,
|
||||||
|
" {} <= {} < {}",
|
||||||
|
self.arg_range[0],
|
||||||
|
self.arg_type,
|
||||||
|
self.arg_range[1]
|
||||||
|
)
|
||||||
};
|
};
|
||||||
|
|
||||||
defmt::write!(fmt, " {} should be", arg_string);
|
|
||||||
|
|
||||||
let inclusive_string = if self.inclusive_upper_bound { "=" } else { "" };
|
|
||||||
|
|
||||||
defmt::write!(
|
|
||||||
fmt,
|
|
||||||
" {} <= {} <{} {}",
|
|
||||||
self.arg_range[0],
|
|
||||||
arg_string,
|
|
||||||
inclusive_string,
|
|
||||||
self.arg_range[1]
|
|
||||||
)
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[derive(Debug)]
|
#[derive(Debug)]
|
||||||
|
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
||||||
pub(super) enum ArgType {
|
pub(super) enum ArgType {
|
||||||
Arg1,
|
Arg1,
|
||||||
Arg2,
|
Arg2,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Input value is out of range for Q1.x format
|
||||||
|
#[allow(missing_docs)]
|
||||||
|
#[derive(Debug)]
|
||||||
|
pub enum NumberOutOfRange {
|
||||||
|
BelowLowerBound,
|
||||||
|
AboveUpperBound,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature = "defmt")]
|
||||||
|
impl defmt::Format for NumberOutOfRange {
|
||||||
|
fn format(&self, fmt: defmt::Formatter) {
|
||||||
|
use NumberOutOfRange::*;
|
||||||
|
|
||||||
|
match self {
|
||||||
|
BelowLowerBound => defmt::write!(fmt, "input value should be equal or greater than -1"),
|
||||||
|
AboveUpperBound => defmt::write!(fmt, "input value should be equal or less than 1"),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -56,7 +56,7 @@ impl Config {
|
||||||
Ok(config)
|
Ok(config)
|
||||||
}
|
}
|
||||||
|
|
||||||
fn check_scale(&self) -> Result<(), CordicError> {
|
fn check_scale(&self) -> Result<(), ConfigError> {
|
||||||
use Function::*;
|
use Function::*;
|
||||||
|
|
||||||
let scale_raw = self.scale as u8;
|
let scale_raw = self.scale as u8;
|
||||||
|
@ -76,10 +76,10 @@ impl Config {
|
||||||
};
|
};
|
||||||
|
|
||||||
if let Some(range) = err_range {
|
if let Some(range) = err_range {
|
||||||
Err(CordicError::ConfigError(ConfigError {
|
Err(ConfigError {
|
||||||
func: self.function,
|
func: self.function,
|
||||||
scale_range: range,
|
scale_range: range,
|
||||||
}))
|
})
|
||||||
} else {
|
} else {
|
||||||
Ok(())
|
Ok(())
|
||||||
}
|
}
|
||||||
|
@ -226,20 +226,20 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
consumed_input_len = double_input.len() + 1;
|
consumed_input_len = double_input.len() + 1;
|
||||||
|
|
||||||
// preload first value from arg1 to cordic
|
// preload first value from arg1 to cordic
|
||||||
self.blocking_write_f64(arg1s[0]);
|
self.blocking_write_f64(arg1s[0])?;
|
||||||
|
|
||||||
for (&arg1, &arg2) in double_input {
|
for (&arg1, &arg2) in double_input {
|
||||||
// Since we manually preload a value before,
|
// Since we manually preload a value before,
|
||||||
// we will write arg2 (from the actual last pair) first, (at this moment, cordic start to calculating,)
|
// we will write arg2 (from the actual last pair) first, (at this moment, cordic start to calculating,)
|
||||||
// and write arg1 (from the actual next pair), then read the result, to "keep preloading"
|
// and write arg1 (from the actual next pair), then read the result, to "keep preloading"
|
||||||
|
|
||||||
self.blocking_write_f64(arg2);
|
self.blocking_write_f64(arg2)?;
|
||||||
self.blocking_write_f64(arg1);
|
self.blocking_write_f64(arg1)?;
|
||||||
self.blocking_read_f64_to_buf(output, &mut output_count);
|
self.blocking_read_f64_to_buf(output, &mut output_count);
|
||||||
}
|
}
|
||||||
|
|
||||||
// write last input value from arg2s, then read out the result
|
// write last input value from arg2s, then read out the result
|
||||||
self.blocking_write_f64(arg2s[arg2s.len() - 1]);
|
self.blocking_write_f64(arg2s[arg2s.len() - 1])?;
|
||||||
self.blocking_read_f64_to_buf(output, &mut output_count);
|
self.blocking_read_f64_to_buf(output, &mut output_count);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -253,12 +253,12 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
self.peri.set_argument_count(AccessCount::One);
|
self.peri.set_argument_count(AccessCount::One);
|
||||||
|
|
||||||
// "preload" value to cordic (at this moment, cordic start to calculating)
|
// "preload" value to cordic (at this moment, cordic start to calculating)
|
||||||
self.blocking_write_f64(input_left[0]);
|
self.blocking_write_f64(input_left[0])?;
|
||||||
|
|
||||||
for &arg in input_left.iter().skip(1) {
|
for &arg in input_left.iter().skip(1) {
|
||||||
// this line write arg for next round caculation to cordic,
|
// this line write arg for next round caculation to cordic,
|
||||||
// and read result from last round
|
// and read result from last round
|
||||||
self.blocking_write_f64(arg);
|
self.blocking_write_f64(arg)?;
|
||||||
self.blocking_read_f64_to_buf(output, &mut output_count);
|
self.blocking_read_f64_to_buf(output, &mut output_count);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -281,8 +281,9 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
fn blocking_write_f64(&mut self, arg: f64) {
|
fn blocking_write_f64(&mut self, arg: f64) -> Result<(), NumberOutOfRange> {
|
||||||
self.peri.write_argument(utils::f64_to_q1_31(arg));
|
self.peri.write_argument(utils::f64_to_q1_31(arg)?);
|
||||||
|
Ok(())
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Run a async CORDIC calculation in q.1.31 format
|
/// Run a async CORDIC calculation in q.1.31 format
|
||||||
|
@ -339,7 +340,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
|
|
||||||
for (&arg1, &arg2) in double_input {
|
for (&arg1, &arg2) in double_input {
|
||||||
for &arg in [arg1, arg2].iter() {
|
for &arg in [arg1, arg2].iter() {
|
||||||
input_buf[input_buf_len] = utils::f64_to_q1_31(arg);
|
input_buf[input_buf_len] = utils::f64_to_q1_31(arg)?;
|
||||||
input_buf_len += 1;
|
input_buf_len += 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -383,7 +384,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
self.peri.set_argument_count(AccessCount::One);
|
self.peri.set_argument_count(AccessCount::One);
|
||||||
|
|
||||||
for &arg in input_remain {
|
for &arg in input_remain {
|
||||||
input_buf[input_buf_len] = utils::f64_to_q1_31(arg);
|
input_buf[input_buf_len] = utils::f64_to_q1_31(arg)?;
|
||||||
input_buf_len += 1;
|
input_buf_len += 1;
|
||||||
|
|
||||||
if input_buf_len == INPUT_BUF_MAX_LEN {
|
if input_buf_len == INPUT_BUF_MAX_LEN {
|
||||||
|
@ -509,10 +510,10 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
let (&arg1, &arg2) = args.next().unwrap();
|
let (&arg1, &arg2) = args.next().unwrap();
|
||||||
|
|
||||||
// preloading 1 pair of arguments
|
// preloading 1 pair of arguments
|
||||||
self.blocking_write_f32(arg1, arg2);
|
self.blocking_write_f32(arg1, arg2)?;
|
||||||
|
|
||||||
for (&arg1, &arg2) in args {
|
for (&arg1, &arg2) in args {
|
||||||
self.blocking_write_f32(arg1, arg2);
|
self.blocking_write_f32(arg1, arg2)?;
|
||||||
self.blocking_read_f32_to_buf(output, &mut output_count);
|
self.blocking_read_f32_to_buf(output, &mut output_count);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -522,15 +523,13 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
Ok(output_count)
|
Ok(output_count)
|
||||||
}
|
}
|
||||||
|
|
||||||
fn blocking_write_f32(&mut self, arg1: f32, arg2: f32) {
|
fn blocking_write_f32(&mut self, arg1: f32, arg2: f32) -> Result<(), NumberOutOfRange> {
|
||||||
let reg_value: u32 = utils::f32_args_to_u32(arg1, arg2);
|
self.peri.write_argument(utils::f32_args_to_u32(arg1, arg2)?);
|
||||||
self.peri.write_argument(reg_value);
|
Ok(())
|
||||||
}
|
}
|
||||||
|
|
||||||
fn blocking_read_f32_to_buf(&mut self, result_buf: &mut [f32], result_index: &mut usize) {
|
fn blocking_read_f32_to_buf(&mut self, result_buf: &mut [f32], result_index: &mut usize) {
|
||||||
let reg_value = self.peri.read_result();
|
let (res1, res2) = utils::u32_to_f32_res(self.peri.read_result());
|
||||||
|
|
||||||
let (res1, res2) = utils::u32_to_f32_res(reg_value);
|
|
||||||
|
|
||||||
result_buf[*result_index] = res1;
|
result_buf[*result_index] = res1;
|
||||||
*result_index += 1;
|
*result_index += 1;
|
||||||
|
@ -597,7 +596,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
);
|
);
|
||||||
|
|
||||||
for (&arg1, &arg2) in args {
|
for (&arg1, &arg2) in args {
|
||||||
input_buf[input_buf_len] = utils::f32_args_to_u32(arg1, arg2);
|
input_buf[input_buf_len] = utils::f32_args_to_u32(arg1, arg2)?;
|
||||||
input_buf_len += 1;
|
input_buf_len += 1;
|
||||||
|
|
||||||
if input_buf_len == INPUT_BUF_MAX_LEN {
|
if input_buf_len == INPUT_BUF_MAX_LEN {
|
||||||
|
@ -655,7 +654,7 @@ impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
macro_rules! check_input_value {
|
macro_rules! check_input_value {
|
||||||
($func_name:ident, $float_type:ty) => {
|
($func_name:ident, $float_type:ty) => {
|
||||||
impl<'d, T: Instance> Cordic<'d, T> {
|
impl<'d, T: Instance> Cordic<'d, T> {
|
||||||
fn $func_name(&self, arg1s: &[$float_type], arg2s: Option<&[$float_type]>) -> Result<(), CordicError> {
|
fn $func_name(&self, arg1s: &[$float_type], arg2s: Option<&[$float_type]>) -> Result<(), ArgError> {
|
||||||
let config = &self.config;
|
let config = &self.config;
|
||||||
|
|
||||||
use Function::*;
|
use Function::*;
|
||||||
|
@ -741,13 +740,13 @@ macro_rules! check_input_value {
|
||||||
};
|
};
|
||||||
|
|
||||||
if let Some(err) = err_info {
|
if let Some(err) = err_info {
|
||||||
return Err(CordicError::ArgError(ArgError {
|
return Err(ArgError {
|
||||||
func: config.function,
|
func: config.function,
|
||||||
scale: err.scale,
|
scale: err.scale,
|
||||||
arg_range: err.range,
|
arg_range: err.range,
|
||||||
inclusive_upper_bound: err.inclusive_upper_bound,
|
inclusive_upper_bound: err.inclusive_upper_bound,
|
||||||
arg_type: ArgType::Arg1,
|
arg_type: ArgType::Arg1,
|
||||||
}));
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
// check ARG2 value
|
// check ARG2 value
|
||||||
|
@ -769,13 +768,13 @@ macro_rules! check_input_value {
|
||||||
};
|
};
|
||||||
|
|
||||||
if let Some(err) = err_info {
|
if let Some(err) = err_info {
|
||||||
return Err(CordicError::ArgError(ArgError {
|
return Err(ArgError {
|
||||||
func: config.function,
|
func: config.function,
|
||||||
scale: None,
|
scale: None,
|
||||||
arg_range: err.range,
|
arg_range: err.range,
|
||||||
inclusive_upper_bound: true,
|
inclusive_upper_bound: true,
|
||||||
arg_type: ArgType::Arg2,
|
arg_type: ArgType::Arg2,
|
||||||
}));
|
});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,39 +1,42 @@
|
||||||
//! Common match utils
|
//! Common match utils
|
||||||
|
use super::errors::NumberOutOfRange;
|
||||||
|
|
||||||
macro_rules! floating_fixed_convert {
|
macro_rules! floating_fixed_convert {
|
||||||
($f_to_q:ident, $q_to_f:ident, $unsigned_bin_typ:ty, $signed_bin_typ:ty, $float_ty:ty, $offset:literal, $min_positive:literal) => {
|
($f_to_q:ident, $q_to_f:ident, $unsigned_bin_typ:ty, $signed_bin_typ:ty, $float_ty:ty, $offset:literal, $min_positive:literal) => {
|
||||||
/// convert float point to fixed point format
|
/// convert float point to fixed point format
|
||||||
pub(crate) fn $f_to_q(value: $float_ty) -> $unsigned_bin_typ {
|
pub fn $f_to_q(value: $float_ty) -> Result<$unsigned_bin_typ, NumberOutOfRange> {
|
||||||
const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) };
|
const MIN_POSITIVE: $float_ty = unsafe { core::mem::transmute($min_positive) };
|
||||||
|
|
||||||
assert!(
|
if value < -1.0 {
|
||||||
(-1.0 as $float_ty) <= value,
|
return Err(NumberOutOfRange::BelowLowerBound)
|
||||||
"input value {} should be equal or greater than -1",
|
}
|
||||||
value
|
|
||||||
);
|
if value > 1.0 {
|
||||||
|
return Err(NumberOutOfRange::AboveUpperBound)
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
let value = if value == 1.0 as $float_ty{
|
let value = if 1.0 - MIN_POSITIVE < value && value <= 1.0 {
|
||||||
// make a exception for user specifing exact 1.0 float point,
|
// make a exception for value between (1.0^{-x} , 1.0] float point,
|
||||||
// convert 1.0 to max representable value of q1.x format
|
// convert it to max representable value of q1.x format
|
||||||
(1.0 as $float_ty) - MIN_POSITIVE
|
(1.0 as $float_ty) - MIN_POSITIVE
|
||||||
} else {
|
} else {
|
||||||
assert!(
|
|
||||||
value <= (1.0 as $float_ty) - MIN_POSITIVE,
|
|
||||||
"input value {} should be equal or less than 1-2^(-{})",
|
|
||||||
value, $offset
|
|
||||||
);
|
|
||||||
value
|
value
|
||||||
};
|
};
|
||||||
|
|
||||||
(value * ((1 as $unsigned_bin_typ << $offset) as $float_ty)) as $unsigned_bin_typ
|
// It's necessary to cast the float value to signed integer, before convert it to a unsigned value.
|
||||||
|
// Since value from register is actually a "signed value", a "as" cast will keep original binary format but mark it as unsgined value.
|
||||||
|
// see https://doc.rust-lang.org/reference/expressions/operator-expr.html#numeric-cast
|
||||||
|
Ok((value * ((1 as $unsigned_bin_typ << $offset) as $float_ty)) as $signed_bin_typ as $unsigned_bin_typ)
|
||||||
}
|
}
|
||||||
|
|
||||||
#[inline(always)]
|
#[inline(always)]
|
||||||
/// convert fixed point to float point format
|
/// convert fixed point to float point format
|
||||||
pub(crate) fn $q_to_f(value: $unsigned_bin_typ) -> $float_ty {
|
pub fn $q_to_f(value: $unsigned_bin_typ) -> $float_ty {
|
||||||
// It's needed to convert from unsigned to signed first, for correct result.
|
// It's necessary to cast the unsigned integer to signed integer, before convert it to a float value.
|
||||||
-(value as $signed_bin_typ as $float_ty) / ((1 as $unsigned_bin_typ << $offset) as $float_ty)
|
// Since value from register is actually a "signed value", a "as" cast will keep original binary format but mark it as signed value.
|
||||||
|
// see https://doc.rust-lang.org/reference/expressions/operator-expr.html#numeric-cast
|
||||||
|
(value as $signed_bin_typ as $float_ty) / ((1 as $unsigned_bin_typ << $offset) as $float_ty)
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
@ -59,8 +62,8 @@ floating_fixed_convert!(
|
||||||
);
|
);
|
||||||
|
|
||||||
#[inline(always)]
|
#[inline(always)]
|
||||||
pub(crate) fn f32_args_to_u32(arg1: f32, arg2: f32) -> u32 {
|
pub(crate) fn f32_args_to_u32(arg1: f32, arg2: f32) -> Result<u32, NumberOutOfRange> {
|
||||||
f32_to_q1_15(arg1) as u32 + ((f32_to_q1_15(arg2) as u32) << 16)
|
Ok(f32_to_q1_15(arg1)? as u32 + ((f32_to_q1_15(arg2)? as u32) << 16))
|
||||||
}
|
}
|
||||||
|
|
||||||
#[inline(always)]
|
#[inline(always)]
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue