Struct PidInstanceF32 

pub struct PidInstanceF32 {
    pub a0: f32,
    pub a1: f32,
    pub a2: f32,
    pub state: [f32; 3],
    pub kp: f32,
    pub ki: f32,
    pub kd: f32,
}

Instance structure for the floating-point PID Control.

Fields

a0: f32

a1: f32

a2: f32

state: [f32; 3]

kp: f32

ki: f32

kd: f32

Implementations

impl PidInstanceF32

pub fn new(kp: f32, ki: f32, kd: f32) -> Self

Examples found in repository

examples/basic_usage.rs (line 36)

fn main() {
    println!("=== embedded-dsp Basic Usage Example ===");

    // 1. Vector Operations
    let a = [1.0f32, 2.0, 3.0, 4.0];
    let b = [10.0f32, 20.0, 30.0, 40.0];
    let mut vec_out = [0.0f32; 4];
    add_f32(&a, &b, &mut vec_out);
    println!("Vector Add: {:?}", vec_out);

    let dot = dot_prod_f32(&a, &b);
    println!("Vector Dot Product: {}", dot);

    // 2. Q15 Fixed-Point Saturating Math
    let q15_a = [20000i16, 25000];
    let q15_b = [15000i16, 10000];
    let mut q15_out = [0i16; 2];
    add_q15(&q15_a, &q15_b, &mut q15_out);
    println!("Q15 Saturating Add (clamped at 32767): {:?}", q15_out);

    // 3. FIR Filtering
    let coeffs = [0.25f32, 0.5, 0.25]; // 3-tap moving average filter
    let mut state = [0.0f32; 3 + 4 - 1];
    let mut fir = FirInstanceF32::init(3, &coeffs, &mut state);

    let input_signal = [1.0f32, 2.0, 3.0, 4.0];
    let mut filtered_signal = [0.0f32; 4];
    fir_f32(&mut fir, &input_signal, &mut filtered_signal);
    println!("FIR Filter Output: {:?}", filtered_signal);

    // 4. PID Motor Controller
    let mut pid = PidInstanceF32::new(2.0, 0.1, 0.05);
    let control_output = pid.process(10.0);
    println!("PID Control Signal: {}", control_output);

    // 5. 64-Point Complex FFT
    let mut fft_data = [0.0f32; 128]; // 64 complex pairs [re, im, ...]
    for i in 0..64 {
        fft_data[2 * i] = (i as f32 * 0.1).sin();
    }
    cfft_f32(&mut fft_data, 64, 0, 1);
    println!("64-Point Complex FFT processed successfully!");

    // 6. 1D Conditional Median Filtering (Impulse / Spike Rejection)
    let spiky_signal = [1.0f32, 1.1, 1.0, 100.0, 1.2, 1.1, 1.0];
    let mut clean_signal = [0.0f32; 7];
    median_filter_1d_f32(&spiky_signal, &mut clean_signal, 3, 5.0);
    println!("Conditional Median Filter Out: {:?}", clean_signal);

    // 7. Welch's Method Power Spectral Density (PSD)
    let mut psd_out = [0.0f32; 32];
    let mut sine_wave = [0.0f32; 128];
    for (i, val) in sine_wave.iter_mut().enumerate() {
        *val = (2.0 * core::f32::consts::PI * 100.0 * (i as f32) / 1000.0).sin();
    }
    welch_psd_f32(
        &sine_wave,
        &mut psd_out,
        64,
        32,
        1000.0,
        WelchWindow::Hamming,
        true,
    );
    println!("Welch PSD (dB) at bin 6: {:.2} dB", psd_out[6]);

    // 8. 2D Spatial Processing (2D DCT & Sobel Edge Detection)
    let img_4x4 = [
        0.0f32, 0.0, 10.0, 10.0, 0.0, 0.0, 10.0, 10.0, 0.0, 0.0, 10.0, 10.0, 0.0, 0.0, 10.0, 10.0,
    ];
    let mut edges = [0.0f32; 16];
    sobel_edge_detection_f32(&img_4x4, &mut edges, 4, 4, 15.0);
    println!("2D Sobel Edge Output (4x4): {:?}", edges);

    // 9. Weighted Polynomial Least-Squares Sensor Calibration
    let x_cal = [0.0f32, 1.0, 2.0, 3.0, 4.0];
    let y_cal = [2.0f32, 5.0, 8.0, 11.0, 14.0]; // y = 2 + 3x
    let mut cal_coeffs = [0.0f32; 2];
    polynomial_least_squares_fit(&x_cal, &y_cal, None, 1, &mut cal_coeffs);
    println!(
        "Fitted Sensor Calibration: y = {:.2} + {:.2}*x",
        cal_coeffs[0], cal_coeffs[1]
    );
}

examples/motor_control_foc.rs (line 186)

fn main() {
    println!("===============================================================================");
    println!("       embedded-dsp Field-Oriented Control (FOC) & Motor Control Loop          ");
    println!("===============================================================================");
    println!();

    const CONTROL_RATE_HZ: f32 = 20000.0; // 20 kHz FOC current loop (50 μs per step)
    const NUM_CONTROL_CYCLES: usize = 20;

    // -----------------------------------------------------------------------------------------
    // 1. 3-Phase Current Feedback Simulation with Inverter Switching Ripple
    // -----------------------------------------------------------------------------------------
    println!("--- 1. 3-Phase Current Sensing with Inverter PWM Switching Noise ---");
    let target_torque_current = 5.0f32; // 5 Amps commanded torque current (Iq)
    let rotor_speed_rpm = 3000.0f32;
    let pole_pairs = 4.0f32;
    let electrical_freq_hz = (rotor_speed_rpm * pole_pairs) / 60.0; // 200 Hz electrical freq
    let omega_e = 2.0 * core::f32::consts::PI * electrical_freq_hz;

    let mut ia_raw = [0.0f32; NUM_CONTROL_CYCLES];
    let mut ib_raw = [0.0f32; NUM_CONTROL_CYCLES];
    let mut ic_raw = [0.0f32; NUM_CONTROL_CYCLES];
    let mut theta_e = [0.0f32; NUM_CONTROL_CYCLES];

    for i in 0..NUM_CONTROL_CYCLES {
        let t = i as f32 / CONTROL_RATE_HZ;
        let theta = (omega_e * t) % (2.0 * core::f32::consts::PI);
        theta_e[i] = theta;

        // Ideal stator currents (balanced 3-phase, 120° apart, Iq = 5A, Id = 0A)
        // Ia = -Iq * sin(θ), Ib = -Iq * sin(θ - 2π/3), Ic = -Iq * sin(θ + 2π/3)
        let ia_ideal = -target_torque_current * theta.sin();
        let ib_ideal = -target_torque_current * (theta - 2.0 * core::f32::consts::PI / 3.0).sin();
        let ic_ideal = -target_torque_current * (theta + 2.0 * core::f32::consts::PI / 3.0).sin();

        // 20 kHz PWM inverter switching noise (±0.4A ripple)
        let ripple = if i % 2 == 0 { 0.35f32 } else { -0.35f32 };
        ia_raw[i] = ia_ideal + ripple;
        ib_raw[i] = ib_ideal - ripple * 0.5;
        ic_raw[i] = ic_ideal - ripple * 0.5;
    }

    println!(
        "  Initial Raw Stator Currents at t=0: Ia={:.2}A, Ib={:.2}A, Ic={:.2}A",
        ia_raw[0], ib_raw[0], ic_raw[0]
    );

    // -----------------------------------------------------------------------------------------
    // 2. Real-Time Current Filtering (O(1) Recursive Moving Average)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 2. Stator Current Feedback Filtering (O(1) Recursive Moving Average) ---");
    let mut ma_filter_ia = RecursiveMovingAverage::<8>::new();
    let mut ma_filter_ib = RecursiveMovingAverage::<8>::new();
    let mut ia_filtered = [0.0f32; NUM_CONTROL_CYCLES];
    let mut ib_filtered = [0.0f32; NUM_CONTROL_CYCLES];

    for i in 0..NUM_CONTROL_CYCLES {
        ia_filtered[i] = ma_filter_ia.process(ia_raw[i]);
        ib_filtered[i] = ma_filter_ib.process(ib_raw[i]);
    }
    println!(
        "  Raw Ia[4] = {:.3}A -> Filtered Ia[4] = {:.3}A",
        ia_raw[4], ia_filtered[4]
    );
    println!(
        "  Raw Ib[4] = {:.3}A -> Filtered Ib[4] = {:.3}A",
        ib_raw[4], ib_filtered[4]
    );

    // -----------------------------------------------------------------------------------------
    // 3. Fast Trigonometric Evaluation & Angle Transformations
    // -----------------------------------------------------------------------------------------
    println!("\n--- 3. Fast Trigonometry vs Table Lookup for Rotor Angle θ ---");
    let test_angle = core::f32::consts::FRAC_PI_3; // 60 degrees (π/3 rad)
    let fast_s_i16 = fast_sin_i16(test_angle);
    let fast_c_i16 = fast_cos_i16(test_angle);
    let std_s = test_angle.sin();
    let std_c = test_angle.cos();
    println!(
        "  θ = 60°: LUT fast_sin_i16 = {} ({:.4}, Exact: {:.4})",
        fast_s_i16,
        fast_s_i16 as f32 / 32768.0,
        std_s
    );
    println!(
        "  θ = 60°: LUT fast_cos_i16 = {} ({:.4}, Exact: {:.4})",
        fast_c_i16,
        fast_c_i16 as f32 / 32768.0,
        std_c
    );

    // Q31 Fixed-Point CORDIC Sine/Cosine (angle in [-π, π) mapped to Q31)
    let angle_q31 = ((test_angle / core::f32::consts::PI) * 2147483648.0) as i32;
    let s_q31 = sin_q31(angle_q31);
    let c_q31 = cos_q31(angle_q31);
    println!(
        "  CORDIC Q31 Sine/Cosine: sin = {} ({:.4}, Exact: {:.4}), cos = {} ({:.4}, Exact: {:.4})",
        s_q31,
        s_q31 as f64 / 2147483648.0,
        std_s,
        c_q31,
        c_q31 as f64 / 2147483648.0,
        std_c
    );

    // -----------------------------------------------------------------------------------------
    // 4. Forward Clarke & Park Transforms (f32 & Q15)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 4. Forward Clarke & Park Transforms ---");
    let mut i_alpha = 0.0f32;
    let mut i_beta = 0.0f32;
    let mut i_d = 0.0f32;
    let mut i_q = 0.0f32;

    // Evaluate step 10
    let step_idx = 10;
    clarke_f32(
        ia_filtered[step_idx],
        ib_filtered[step_idx],
        &mut i_alpha,
        &mut i_beta,
    );
    park_f32(i_alpha, i_beta, theta_e[step_idx], &mut i_d, &mut i_q);

    println!("  f32 Forward Transforms at Step {}:", step_idx);
    println!(
        "    • Stator Stationary Frame: I_alpha = {:>6.3} A, I_beta = {:>6.3} A",
        i_alpha, i_beta
    );
    println!(
        "    • Rotor Synchronous Frame: I_d (Flux) = {:>6.3} A, I_q (Torque) = {:>6.3} A",
        i_d, i_q
    );

    // Fixed-point Q15 Clarke and Park verification
    let q15_scale = 1000.0f32; // 1 Amp = 1000 Q15 counts
    let ia_q15 = (ia_filtered[step_idx] * q15_scale) as q15;
    let ib_q15 = (ib_filtered[step_idx] * q15_scale) as q15;
    let mut q15_alpha = 0i16;
    let mut q15_beta = 0i16;
    let mut q15_d = 0i16;
    let mut q15_q = 0i16;

    clarke_q15(ia_q15, ib_q15, &mut q15_alpha, &mut q15_beta);
    let sin_q15 = fast_sin_i16(theta_e[step_idx]);
    let cos_q15 = fast_cos_i16(theta_e[step_idx]);
    park_q15(
        q15_alpha, q15_beta, sin_q15, cos_q15, &mut q15_d, &mut q15_q,
    );

    println!("  Q15 Forward Transforms:");
    println!(
        "    • Q15 Clarke: alpha = {} ({:.3} A), beta = {} ({:.3} A)",
        q15_alpha,
        q15_alpha as f32 / q15_scale,
        q15_beta,
        q15_beta as f32 / q15_scale
    );
    println!(
        "    • Q15 Park  : d = {} ({:.3} A), q = {} ({:.3} A)",
        q15_d,
        q15_d as f32 / q15_scale,
        q15_q,
        q15_q as f32 / q15_scale
    );

    // -----------------------------------------------------------------------------------------
    // 5. Dual Current Regulators (Id & Iq PID Loops) + Outer Speed Controller
    // -----------------------------------------------------------------------------------------
    println!("\n--- 5. Vector Current Regulators (Dual PID Loops for Id & Iq) ---");
    // Outer Velocity Loop: Target 3000 RPM, Actual 2950 RPM -> Error = 50 RPM
    let mut speed_pid = PidInstanceF32::new(0.08, 0.005, 0.001);
    let speed_error_rpm = 50.0f32;
    let demanded_iq = speed_pid.process(speed_error_rpm).clamp(-15.0, 15.0);
    println!(
        "  Outer Speed PID: Error = {:.1} RPM -> Commanded I_q* = {:.3} A",
        speed_error_rpm, demanded_iq
    );

    // Inner Current Regulators:
    // Id controller: Setpoint = 0.0 A (Zero d-axis current for Maximum Torque Per Ampere)
    // Iq controller: Setpoint = demanded_iq
    let mut id_pid = PidInstanceF32::new(2.5, 0.15, 0.0);
    let mut iq_pid = PidInstanceF32::new(2.5, 0.15, 0.0);

    let id_setpoint = 0.0f32;
    let iq_setpoint = demanded_iq;

    let id_error = id_setpoint - i_d;
    let iq_error = iq_setpoint - i_q;

    let v_d_command = id_pid.process(id_error).clamp(-24.0, 24.0); // 24V DC bus voltage limit
    let v_q_command = iq_pid.process(iq_error).clamp(-24.0, 24.0);

    println!("  Inner Current PIDs:");
    println!(
        "    • d-Axis (Flux)  : Error = {:>6.3} A -> Commanded V_d = {:>6.3} V",
        id_error, v_d_command
    );
    println!(
        "    • q-Axis (Torque): Error = {:>6.3} A -> Commanded V_q = {:>6.3} V",
        iq_error, v_q_command
    );

    // -----------------------------------------------------------------------------------------
    // 6. Inverse Park & Inverse Clarke Transforms (Modulation Voltages)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 6. Inverse Park & Inverse Clarke Transforms (SVPWM Modulation) ---");
    let mut v_alpha = 0.0f32;
    let mut v_beta = 0.0f32;
    let mut v_a = 0.0f32;
    let mut v_b = 0.0f32;

    inv_park_f32(
        v_d_command,
        v_q_command,
        theta_e[step_idx],
        &mut v_alpha,
        &mut v_beta,
    );
    inv_clarke_f32(v_alpha, v_beta, &mut v_a, &mut v_b);
    let v_c = -v_a - v_b; // 3-phase balanced neutral

    println!(
        "  Inverse Park   : V_alpha = {:>6.3} V, V_beta = {:>6.3} V",
        v_alpha, v_beta
    );
    println!(
        "  Inverse Clarke : V_a = {:>6.3} V, V_b = {:>6.3} V, V_c = {:>6.3} V",
        v_a, v_b, v_c
    );

    // Compute PWM Duty Cycles for Inverter Gate Drivers (normalized to 0.0 .. 1.0)
    let v_dc = 24.0f32;
    let duty_a = (v_a / v_dc + 0.5).clamp(0.0, 1.0);
    let duty_b = (v_b / v_dc + 0.5).clamp(0.0, 1.0);
    let duty_c = (v_c / v_dc + 0.5).clamp(0.0, 1.0);

    println!("  Generated Inverter PWM Duty Cycles:");
    println!("    • Phase A Duty : {:>5.1} %", duty_a * 100.0);
    println!("    • Phase B Duty : {:>5.1} %", duty_b * 100.0);
    println!("    • Phase C Duty : {:>5.1} %", duty_c * 100.0);

    println!();
    println!("===============================================================================");
    println!("             Field-Oriented Control (FOC) Execution Complete!                  ");
    println!("===============================================================================");
}

pub fn init(&mut self, reset_state_flag: i32)

pub fn reset(&mut self)

pub fn process(&mut self, in_val: f32) -> f32

Examples found in repository

examples/basic_usage.rs (line 37)

fn main() {
    println!("=== embedded-dsp Basic Usage Example ===");

    // 1. Vector Operations
    let a = [1.0f32, 2.0, 3.0, 4.0];
    let b = [10.0f32, 20.0, 30.0, 40.0];
    let mut vec_out = [0.0f32; 4];
    add_f32(&a, &b, &mut vec_out);
    println!("Vector Add: {:?}", vec_out);

    let dot = dot_prod_f32(&a, &b);
    println!("Vector Dot Product: {}", dot);

    // 2. Q15 Fixed-Point Saturating Math
    let q15_a = [20000i16, 25000];
    let q15_b = [15000i16, 10000];
    let mut q15_out = [0i16; 2];
    add_q15(&q15_a, &q15_b, &mut q15_out);
    println!("Q15 Saturating Add (clamped at 32767): {:?}", q15_out);

    // 3. FIR Filtering
    let coeffs = [0.25f32, 0.5, 0.25]; // 3-tap moving average filter
    let mut state = [0.0f32; 3 + 4 - 1];
    let mut fir = FirInstanceF32::init(3, &coeffs, &mut state);

    let input_signal = [1.0f32, 2.0, 3.0, 4.0];
    let mut filtered_signal = [0.0f32; 4];
    fir_f32(&mut fir, &input_signal, &mut filtered_signal);
    println!("FIR Filter Output: {:?}", filtered_signal);

    // 4. PID Motor Controller
    let mut pid = PidInstanceF32::new(2.0, 0.1, 0.05);
    let control_output = pid.process(10.0);
    println!("PID Control Signal: {}", control_output);

    // 5. 64-Point Complex FFT
    let mut fft_data = [0.0f32; 128]; // 64 complex pairs [re, im, ...]
    for i in 0..64 {
        fft_data[2 * i] = (i as f32 * 0.1).sin();
    }
    cfft_f32(&mut fft_data, 64, 0, 1);
    println!("64-Point Complex FFT processed successfully!");

    // 6. 1D Conditional Median Filtering (Impulse / Spike Rejection)
    let spiky_signal = [1.0f32, 1.1, 1.0, 100.0, 1.2, 1.1, 1.0];
    let mut clean_signal = [0.0f32; 7];
    median_filter_1d_f32(&spiky_signal, &mut clean_signal, 3, 5.0);
    println!("Conditional Median Filter Out: {:?}", clean_signal);

    // 7. Welch's Method Power Spectral Density (PSD)
    let mut psd_out = [0.0f32; 32];
    let mut sine_wave = [0.0f32; 128];
    for (i, val) in sine_wave.iter_mut().enumerate() {
        *val = (2.0 * core::f32::consts::PI * 100.0 * (i as f32) / 1000.0).sin();
    }
    welch_psd_f32(
        &sine_wave,
        &mut psd_out,
        64,
        32,
        1000.0,
        WelchWindow::Hamming,
        true,
    );
    println!("Welch PSD (dB) at bin 6: {:.2} dB", psd_out[6]);

    // 8. 2D Spatial Processing (2D DCT & Sobel Edge Detection)
    let img_4x4 = [
        0.0f32, 0.0, 10.0, 10.0, 0.0, 0.0, 10.0, 10.0, 0.0, 0.0, 10.0, 10.0, 0.0, 0.0, 10.0, 10.0,
    ];
    let mut edges = [0.0f32; 16];
    sobel_edge_detection_f32(&img_4x4, &mut edges, 4, 4, 15.0);
    println!("2D Sobel Edge Output (4x4): {:?}", edges);

    // 9. Weighted Polynomial Least-Squares Sensor Calibration
    let x_cal = [0.0f32, 1.0, 2.0, 3.0, 4.0];
    let y_cal = [2.0f32, 5.0, 8.0, 11.0, 14.0]; // y = 2 + 3x
    let mut cal_coeffs = [0.0f32; 2];
    polynomial_least_squares_fit(&x_cal, &y_cal, None, 1, &mut cal_coeffs);
    println!(
        "Fitted Sensor Calibration: y = {:.2} + {:.2}*x",
        cal_coeffs[0], cal_coeffs[1]
    );
}

examples/motor_control_foc.rs (line 188)

fn main() {
    println!("===============================================================================");
    println!("       embedded-dsp Field-Oriented Control (FOC) & Motor Control Loop          ");
    println!("===============================================================================");
    println!();

    const CONTROL_RATE_HZ: f32 = 20000.0; // 20 kHz FOC current loop (50 μs per step)
    const NUM_CONTROL_CYCLES: usize = 20;

    // -----------------------------------------------------------------------------------------
    // 1. 3-Phase Current Feedback Simulation with Inverter Switching Ripple
    // -----------------------------------------------------------------------------------------
    println!("--- 1. 3-Phase Current Sensing with Inverter PWM Switching Noise ---");
    let target_torque_current = 5.0f32; // 5 Amps commanded torque current (Iq)
    let rotor_speed_rpm = 3000.0f32;
    let pole_pairs = 4.0f32;
    let electrical_freq_hz = (rotor_speed_rpm * pole_pairs) / 60.0; // 200 Hz electrical freq
    let omega_e = 2.0 * core::f32::consts::PI * electrical_freq_hz;

    let mut ia_raw = [0.0f32; NUM_CONTROL_CYCLES];
    let mut ib_raw = [0.0f32; NUM_CONTROL_CYCLES];
    let mut ic_raw = [0.0f32; NUM_CONTROL_CYCLES];
    let mut theta_e = [0.0f32; NUM_CONTROL_CYCLES];

    for i in 0..NUM_CONTROL_CYCLES {
        let t = i as f32 / CONTROL_RATE_HZ;
        let theta = (omega_e * t) % (2.0 * core::f32::consts::PI);
        theta_e[i] = theta;

        // Ideal stator currents (balanced 3-phase, 120° apart, Iq = 5A, Id = 0A)
        // Ia = -Iq * sin(θ), Ib = -Iq * sin(θ - 2π/3), Ic = -Iq * sin(θ + 2π/3)
        let ia_ideal = -target_torque_current * theta.sin();
        let ib_ideal = -target_torque_current * (theta - 2.0 * core::f32::consts::PI / 3.0).sin();
        let ic_ideal = -target_torque_current * (theta + 2.0 * core::f32::consts::PI / 3.0).sin();

        // 20 kHz PWM inverter switching noise (±0.4A ripple)
        let ripple = if i % 2 == 0 { 0.35f32 } else { -0.35f32 };
        ia_raw[i] = ia_ideal + ripple;
        ib_raw[i] = ib_ideal - ripple * 0.5;
        ic_raw[i] = ic_ideal - ripple * 0.5;
    }

    println!(
        "  Initial Raw Stator Currents at t=0: Ia={:.2}A, Ib={:.2}A, Ic={:.2}A",
        ia_raw[0], ib_raw[0], ic_raw[0]
    );

    // -----------------------------------------------------------------------------------------
    // 2. Real-Time Current Filtering (O(1) Recursive Moving Average)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 2. Stator Current Feedback Filtering (O(1) Recursive Moving Average) ---");
    let mut ma_filter_ia = RecursiveMovingAverage::<8>::new();
    let mut ma_filter_ib = RecursiveMovingAverage::<8>::new();
    let mut ia_filtered = [0.0f32; NUM_CONTROL_CYCLES];
    let mut ib_filtered = [0.0f32; NUM_CONTROL_CYCLES];

    for i in 0..NUM_CONTROL_CYCLES {
        ia_filtered[i] = ma_filter_ia.process(ia_raw[i]);
        ib_filtered[i] = ma_filter_ib.process(ib_raw[i]);
    }
    println!(
        "  Raw Ia[4] = {:.3}A -> Filtered Ia[4] = {:.3}A",
        ia_raw[4], ia_filtered[4]
    );
    println!(
        "  Raw Ib[4] = {:.3}A -> Filtered Ib[4] = {:.3}A",
        ib_raw[4], ib_filtered[4]
    );

    // -----------------------------------------------------------------------------------------
    // 3. Fast Trigonometric Evaluation & Angle Transformations
    // -----------------------------------------------------------------------------------------
    println!("\n--- 3. Fast Trigonometry vs Table Lookup for Rotor Angle θ ---");
    let test_angle = core::f32::consts::FRAC_PI_3; // 60 degrees (π/3 rad)
    let fast_s_i16 = fast_sin_i16(test_angle);
    let fast_c_i16 = fast_cos_i16(test_angle);
    let std_s = test_angle.sin();
    let std_c = test_angle.cos();
    println!(
        "  θ = 60°: LUT fast_sin_i16 = {} ({:.4}, Exact: {:.4})",
        fast_s_i16,
        fast_s_i16 as f32 / 32768.0,
        std_s
    );
    println!(
        "  θ = 60°: LUT fast_cos_i16 = {} ({:.4}, Exact: {:.4})",
        fast_c_i16,
        fast_c_i16 as f32 / 32768.0,
        std_c
    );

    // Q31 Fixed-Point CORDIC Sine/Cosine (angle in [-π, π) mapped to Q31)
    let angle_q31 = ((test_angle / core::f32::consts::PI) * 2147483648.0) as i32;
    let s_q31 = sin_q31(angle_q31);
    let c_q31 = cos_q31(angle_q31);
    println!(
        "  CORDIC Q31 Sine/Cosine: sin = {} ({:.4}, Exact: {:.4}), cos = {} ({:.4}, Exact: {:.4})",
        s_q31,
        s_q31 as f64 / 2147483648.0,
        std_s,
        c_q31,
        c_q31 as f64 / 2147483648.0,
        std_c
    );

    // -----------------------------------------------------------------------------------------
    // 4. Forward Clarke & Park Transforms (f32 & Q15)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 4. Forward Clarke & Park Transforms ---");
    let mut i_alpha = 0.0f32;
    let mut i_beta = 0.0f32;
    let mut i_d = 0.0f32;
    let mut i_q = 0.0f32;

    // Evaluate step 10
    let step_idx = 10;
    clarke_f32(
        ia_filtered[step_idx],
        ib_filtered[step_idx],
        &mut i_alpha,
        &mut i_beta,
    );
    park_f32(i_alpha, i_beta, theta_e[step_idx], &mut i_d, &mut i_q);

    println!("  f32 Forward Transforms at Step {}:", step_idx);
    println!(
        "    • Stator Stationary Frame: I_alpha = {:>6.3} A, I_beta = {:>6.3} A",
        i_alpha, i_beta
    );
    println!(
        "    • Rotor Synchronous Frame: I_d (Flux) = {:>6.3} A, I_q (Torque) = {:>6.3} A",
        i_d, i_q
    );

    // Fixed-point Q15 Clarke and Park verification
    let q15_scale = 1000.0f32; // 1 Amp = 1000 Q15 counts
    let ia_q15 = (ia_filtered[step_idx] * q15_scale) as q15;
    let ib_q15 = (ib_filtered[step_idx] * q15_scale) as q15;
    let mut q15_alpha = 0i16;
    let mut q15_beta = 0i16;
    let mut q15_d = 0i16;
    let mut q15_q = 0i16;

    clarke_q15(ia_q15, ib_q15, &mut q15_alpha, &mut q15_beta);
    let sin_q15 = fast_sin_i16(theta_e[step_idx]);
    let cos_q15 = fast_cos_i16(theta_e[step_idx]);
    park_q15(
        q15_alpha, q15_beta, sin_q15, cos_q15, &mut q15_d, &mut q15_q,
    );

    println!("  Q15 Forward Transforms:");
    println!(
        "    • Q15 Clarke: alpha = {} ({:.3} A), beta = {} ({:.3} A)",
        q15_alpha,
        q15_alpha as f32 / q15_scale,
        q15_beta,
        q15_beta as f32 / q15_scale
    );
    println!(
        "    • Q15 Park  : d = {} ({:.3} A), q = {} ({:.3} A)",
        q15_d,
        q15_d as f32 / q15_scale,
        q15_q,
        q15_q as f32 / q15_scale
    );

    // -----------------------------------------------------------------------------------------
    // 5. Dual Current Regulators (Id & Iq PID Loops) + Outer Speed Controller
    // -----------------------------------------------------------------------------------------
    println!("\n--- 5. Vector Current Regulators (Dual PID Loops for Id & Iq) ---");
    // Outer Velocity Loop: Target 3000 RPM, Actual 2950 RPM -> Error = 50 RPM
    let mut speed_pid = PidInstanceF32::new(0.08, 0.005, 0.001);
    let speed_error_rpm = 50.0f32;
    let demanded_iq = speed_pid.process(speed_error_rpm).clamp(-15.0, 15.0);
    println!(
        "  Outer Speed PID: Error = {:.1} RPM -> Commanded I_q* = {:.3} A",
        speed_error_rpm, demanded_iq
    );

    // Inner Current Regulators:
    // Id controller: Setpoint = 0.0 A (Zero d-axis current for Maximum Torque Per Ampere)
    // Iq controller: Setpoint = demanded_iq
    let mut id_pid = PidInstanceF32::new(2.5, 0.15, 0.0);
    let mut iq_pid = PidInstanceF32::new(2.5, 0.15, 0.0);

    let id_setpoint = 0.0f32;
    let iq_setpoint = demanded_iq;

    let id_error = id_setpoint - i_d;
    let iq_error = iq_setpoint - i_q;

    let v_d_command = id_pid.process(id_error).clamp(-24.0, 24.0); // 24V DC bus voltage limit
    let v_q_command = iq_pid.process(iq_error).clamp(-24.0, 24.0);

    println!("  Inner Current PIDs:");
    println!(
        "    • d-Axis (Flux)  : Error = {:>6.3} A -> Commanded V_d = {:>6.3} V",
        id_error, v_d_command
    );
    println!(
        "    • q-Axis (Torque): Error = {:>6.3} A -> Commanded V_q = {:>6.3} V",
        iq_error, v_q_command
    );

    // -----------------------------------------------------------------------------------------
    // 6. Inverse Park & Inverse Clarke Transforms (Modulation Voltages)
    // -----------------------------------------------------------------------------------------
    println!("\n--- 6. Inverse Park & Inverse Clarke Transforms (SVPWM Modulation) ---");
    let mut v_alpha = 0.0f32;
    let mut v_beta = 0.0f32;
    let mut v_a = 0.0f32;
    let mut v_b = 0.0f32;

    inv_park_f32(
        v_d_command,
        v_q_command,
        theta_e[step_idx],
        &mut v_alpha,
        &mut v_beta,
    );
    inv_clarke_f32(v_alpha, v_beta, &mut v_a, &mut v_b);
    let v_c = -v_a - v_b; // 3-phase balanced neutral

    println!(
        "  Inverse Park   : V_alpha = {:>6.3} V, V_beta = {:>6.3} V",
        v_alpha, v_beta
    );
    println!(
        "  Inverse Clarke : V_a = {:>6.3} V, V_b = {:>6.3} V, V_c = {:>6.3} V",
        v_a, v_b, v_c
    );

    // Compute PWM Duty Cycles for Inverter Gate Drivers (normalized to 0.0 .. 1.0)
    let v_dc = 24.0f32;
    let duty_a = (v_a / v_dc + 0.5).clamp(0.0, 1.0);
    let duty_b = (v_b / v_dc + 0.5).clamp(0.0, 1.0);
    let duty_c = (v_c / v_dc + 0.5).clamp(0.0, 1.0);

    println!("  Generated Inverter PWM Duty Cycles:");
    println!("    • Phase A Duty : {:>5.1} %", duty_a * 100.0);
    println!("    • Phase B Duty : {:>5.1} %", duty_b * 100.0);
    println!("    • Phase C Duty : {:>5.1} %", duty_c * 100.0);

    println!();
    println!("===============================================================================");
    println!("             Field-Oriented Control (FOC) Execution Complete!                  ");
    println!("===============================================================================");
}

Trait Implementations

impl Clone for PidInstanceF32

fn clone(&self) -> PidInstanceF32

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for PidInstanceF32

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl Default for PidInstanceF32

fn default() -> PidInstanceF32

Returns the “default value” for a type.

impl DspNode<f32> for PidInstanceF32

Available on crate feature controller only.

fn process_sample(&mut self, input: f32) -> f32

Process a single input sample and produce one output sample.

fn process_block(&mut self, in_buf: &[T], out_buf: &mut [T])

Process a block of samples from in_buf into out_buf.

fn process_in_place(&mut self, buf: &mut [T])

Process a block of samples in place.

fn then<Next>(self, next: Next) -> Chain<Self, Next>

where Self: Sized, Next: DspNode<T>,

Chains this node with another processing node into a sequential pipeline.

impl PartialEq for PidInstanceF32

fn eq(&self, other: &PidInstanceF32) -> bool

Equality operator ==.

fn ne(&self, other: &Rhs) -> bool

Inequality operator !=.

impl StructuralPartialEq for PidInstanceF32