idsp/
cossin.rs

1include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
2
3/// Compute the cosine and sine of an angle.
4/// This is ported from the MiSoC cossin core.
5/// <https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py>
6///
7/// # Arguments
8/// * `phase` - 32-bit phase where `i32::MIN` is -π and `i32::MAX` is π
9///
10/// # Returns
11/// The cos and sin values of the provided phase as a `(i32, i32)`
12/// tuple. With a 7-bit deep LUT there is 9e-6 max and 4e-6 RMS error
13/// in each quadrature over 20 bit phase.
14pub fn cossin(mut phase: i32) -> (i32, i32) {
15    let mut octant = phase as u32;
16    if octant & (1 << 29) != 0 {
17        // phase = pi/4 - phase
18        phase = !phase;
19    }
20
21    // 16 + 1 bits for cos/sin and 15 for dphi to saturate the i32 range.
22    const ALIGN_MSB: usize = 32 - 16 - 1;
23
24    // Mask off octant bits. This leaves the angle in the range [0, pi/4).
25    phase = (((phase as u32) << 3) >> (32 - COSSIN_DEPTH - ALIGN_MSB)) as _;
26
27    let lookup = COSSIN[(phase >> ALIGN_MSB) as usize];
28    phase &= (1 << ALIGN_MSB) - 1;
29
30    // The phase values used for the LUT are at midpoint for the truncated phase.
31    // Interpolate relative to the LUT entry midpoint.
32    phase -= 1 << (ALIGN_MSB - 1);
33
34    // Cancel the -1 bias that was conditionally introduced above.
35    // This lowers the DC spur from 2e-8 to 2e-10 magnitude.
36    // phase += (octant & 1) as i32;
37
38    // Fixed point pi/4.
39    const PI4: i32 = (core::f64::consts::FRAC_PI_4 * (1 << 16) as f64) as _;
40    // No rounding bias necessary here since we keep enough low bits.
41    let dphi = (phase * PI4) >> 16;
42
43    // 1/2 < cos(0 <= x <= pi/4) <= 1: Shift the cos
44    // values and scale the sine values as encoded in the LUT.
45    let mut cos = (lookup & 0xffff) as i32 + (1 << 16);
46    let mut sin = (lookup >> 16) as i32;
47
48    let dcos = (sin * dphi) >> COSSIN_DEPTH;
49    let dsin = (cos * dphi) >> (COSSIN_DEPTH + 1);
50
51    cos = (cos << (ALIGN_MSB - 1)) - dcos;
52    sin = (sin << ALIGN_MSB) + dsin;
53
54    // Unmap using octant bits.
55    octant ^= octant >> 1;
56    if octant & (1 << 29) != 0 {
57        (cos, sin) = (sin, cos);
58    }
59    if octant & (1 << 30) != 0 {
60        cos = -cos;
61    }
62    if octant & (1 << 31) != 0 {
63        sin = -sin;
64    }
65
66    (cos, sin)
67}
68
69#[cfg(test)]
70mod tests {
71    use super::*;
72    use core::f64::consts::PI;
73
74    #[test]
75    fn cossin_error_max_rms_all_phase() {
76        // Constant amplitude error due to LUT data range.
77        const AMPLITUDE: f64 = (1i64 << 31) as f64 - 0.85 * (1i64 << 15) as f64;
78        const MAX_PHASE: f64 = (1i64 << 32) as _;
79        let mut rms_err = (0f64, 0f64);
80        let mut sum_err = (0f64, 0f64);
81        let mut max_err = (0f64, 0f64);
82        let mut sum = (0f64, 0f64);
83        let mut demod = (0f64, 0f64);
84
85        // use std::{fs::File, io::{BufWriter, prelude::*}, path::Path};
86        // let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap());
87
88        // log2 of the number of phase values to check
89        const PHASE_DEPTH: usize = 20;
90
91        for phase in 0..(1 << PHASE_DEPTH) {
92            let phase = (phase << (32 - PHASE_DEPTH)) as i32;
93            let have = cossin(phase);
94            // file.write(&have.0.to_le_bytes()).unwrap();
95            // file.write(&have.1.to_le_bytes()).unwrap();
96
97            let have = (have.0 as f64 / AMPLITUDE, have.1 as f64 / AMPLITUDE);
98
99            let radian_phase = 2. * PI * phase as f64 / MAX_PHASE;
100            let want = (radian_phase.cos(), radian_phase.sin());
101
102            sum.0 += have.0;
103            sum.1 += have.1;
104
105            demod.0 += have.0 * want.0 - have.1 * want.1;
106            demod.1 += have.1 * want.0 + have.0 * want.1;
107
108            let err = (have.0 - want.0, have.1 - want.1);
109
110            sum_err.0 += err.0;
111            sum_err.1 += err.1;
112
113            rms_err.0 += err.0 * err.0;
114            rms_err.1 += err.1 * err.1;
115
116            max_err.0 = max_err.0.max(err.0.abs());
117            max_err.1 = max_err.1.max(err.1.abs());
118        }
119        rms_err.0 /= (1 << PHASE_DEPTH) as f64;
120        rms_err.1 /= (1 << PHASE_DEPTH) as f64;
121
122        println!("sum: {:.2e} {:.2e}", sum.0, sum.1);
123        println!("demod: {:.2e} {:.2e}", demod.0, demod.1);
124        println!("sum_err: {:.2e} {:.2e}", sum_err.0, sum_err.1);
125        println!("rms: {:.2e} {:.2e}", rms_err.0.sqrt(), rms_err.1.sqrt());
126        println!("max: {:.2e} {:.2e}", max_err.0, max_err.1);
127
128        assert!(sum.0.abs() < 4e-10);
129        assert!(sum.1.abs() < 3e-8);
130
131        assert!(demod.0.abs() < 4e-10);
132        assert!(demod.1.abs() < 1e-8);
133
134        assert!(sum_err.0.abs() < 4e-10);
135        assert!(sum_err.1.abs() < 4e-10);
136
137        assert!(rms_err.0.sqrt() < 4e-6);
138        assert!(rms_err.1.sqrt() < 4e-6);
139
140        assert!(max_err.0 < 1e-5);
141        assert!(max_err.1 < 1e-5);
142    }
143}
idsp/cossin.rs

idsp/
cossin.rs
