Fixed-Point [no_std] Sin/Cos calculation

code review
1

Here is my code. I am not including the SIN_TABLE for brevity, but the implementation is based off of the CMSIS arm_sin_cos_q31 function, so the values are the same as in arm_common_tables.c.

The code works, but I am keen to see if the interpolate function could be improved. It is hard to follow and I'm not sure how well the compiler would optimize with SIMD. The intended application is for use in ARM M4 microcontrollers.

use crate::common_tables::SIN_TABLE;

#[derive(Debug)]
pub struct SinCos{
	pub sin: i32,
	pub cos:i32
}


const DN: i32 = 0x1921FB5; // delta between the two points (fixed), in this case 2*pi/FAST_MATH_TABLE_SIZE
const CONTROLLER_Q31_SHIFT: u32 = 32 - 9;

#[inline(always)]
fn clip_i64_to_i32(x: i64) -> i32{
	if x > (i32::MAX as i64) {return i32::MAX;}
	if x < (i32::MIN as i64) {return i32::MIN;}
	x as i32
}

#[inline(always)]
fn interpolate(f1: i32, f2: i32, d1: i32, d2: i32, fract: i32) -> i32{
	let df = f2 - f1;
	let mut temp: i64 = (DN as i64)*((d1 as i64) + (d2 as i64));
	temp = temp - ((df as i64) << 32);
	temp = (fract as i64)*(temp >> 31);
	temp = temp + ((3*(df as i64) << 31) - ((d2 as i64) + ((d1 as i64) << 1))*(DN as i64));
	temp = (fract as i64)*(temp >> 31);
	temp = temp + (d1 as i64)*(DN as i64);
	temp = (fract as i64)*(temp >> 31);
	return clip_i64_to_i32((temp >> 31) + (f1 as i64));
}

impl SinCos{
	pub fn from_theta(theta: i32) -> Self{
		/* Calculate the nearest index */
	  	let index_s:u16 = ((theta as u32) >> CONTROLLER_Q31_SHIFT) as u16;
	  	let index_c:u16 = (index_s + 128) & 0x1ff;

		/* Calculation of fractional value */
	  	let fract: i32 = (theta - ((index_s as i32) << CONTROLLER_Q31_SHIFT)) << 8;

		/* Calculation of cosine value */
		let mut f1: i32 = SIN_TABLE[(index_c+0) as usize] as i32;
		let mut f2: i32 = SIN_TABLE[(index_c+1) as usize] as i32;
		let mut d1: i32 = (SIN_TABLE[(index_s+0) as usize] as i32).wrapping_neg();
		let mut d2: i32 = (SIN_TABLE[(index_s+1) as usize] as i32).wrapping_neg();
		let cos_val = interpolate(f1, f2, d1, d2, fract);

		/* Calculation of sine value */
		f1 = SIN_TABLE[(index_s+0) as usize] as i32;
		f2 = SIN_TABLE[(index_s+1) as usize] as i32;
		d1 = (SIN_TABLE[(index_c+0) as usize] as i32).wrapping_neg();
		d2 = (SIN_TABLE[(index_c+1) as usize] as i32).wrapping_neg();
		let sin_val = interpolate(f1, f2, d1, d2, fract);

		Self{sin:sin_val, cos:cos_val}
	}
}
2

Maybe not what you want to hear -- if you're doing this for fun, or to learn how to do this sort of thing well, then -- bully for you!

If your main goal is to get something else working, then make a Rust wrapper for the CMSIS arm_sin_cos_q31 function.

3

Hey @TimWescott,

I have reasons to do this in pure rust that I frankly wasn't expecting to have to rationalize on this forum such as:

  1. Using generics to support other formats including floating point and "Fixed" formats from the fixed crate.
  2. Adding support for other trig identities as traits.

Anyways. The code is working nicely, so no need to go backwards. I just want feedback from the community on ways to improve, especially on performance in [no_std] environments like ARM M4.

4

Optimizing this sort of thing is very time-intensive (I've done it, if not for ARM processors), and it takes a long time to get your head wrapped around a particular processor's way of doing things. Which is why I suggested what I did, on the rationale that ARM can spend the money doing that optimization, then amortize it over millions of processors sold.

There's no reason you can't do what you called out in a library that itself calls the CMSIS library. Doing so would make a nice extension to that library, and if done right, would wrap the thing in a safe and fast way. It's basically what numerical libraries have been doing ever since someone decided that maybe they wanted to do some number-crunching without learning FORTRAN* -- and BLAS and LAPACK are still written in Fortran under the hood, and have libraries available for half the computer languages in the world, so it can't be that bad of an approach.

If you do not, indeed, want to go "backwards" into something that may well be considerably faster and is certainly much better tested than what any mortal human could do as a one-person project, then I suggest that you dig as deep as you can into the CMSIS source code, and where that stops, run some of those functions on a debugger that lets you trace things in assembly. Then compare what you find to what you're doing. You'll be learning from the experts, so it'll be time reasonably well spent.

* Back when FORTRAN was, indeed, spelled in all caps.

5

This topic was automatically closed 90 days after the last reply. We invite you to open a new topic if you have further questions or comments.