embassy/examples/nrf/src/bin/i2s_waveform.rs

#![no_std]
#![no_main]
#![feature(type_alias_impl_trait)]

use core::f32::consts::PI;

use defmt::{error, info};
use embassy_executor::Spawner;
use embassy_nrf::i2s::{self, Channels, Config, MasterClock, Sample as _, SampleWidth, I2S};
use embassy_nrf::interrupt;
use {defmt_rtt as _, panic_probe as _};

type Sample = i16;

const NUM_SAMPLES: usize = 6000;

#[embassy_executor::main]
async fn main(_spawner: Spawner) {
    let p = embassy_nrf::init(Default::default());

    let master_clock: MasterClock = i2s::ExactSampleRate::_50000.into();

    let sample_rate = master_clock.sample_rate();
    info!("Sample rate: {}", sample_rate);

    let config = Config::default()
        .sample_width(SampleWidth::_16bit)
        .channels(Channels::MonoLeft);

    let irq = interrupt::take!(I2S);
    let mut output_stream = I2S::master(p.I2S, irq, p.P0_25, p.P0_26, p.P0_27, master_clock, config).output(p.P0_28);

    let mut buffers: [i2s::AlignedBuffer<Sample, NUM_SAMPLES>; 3] = [
        i2s::AlignedBuffer::default(),
        i2s::AlignedBuffer::default(),
        i2s::AlignedBuffer::default(),
    ];

    let mut waveform = Waveform::new(1.0 / sample_rate as f32);

    waveform.process(&mut buffers[0]);
    waveform.process(&mut buffers[1]);

    output_stream.start(&buffers[0]).await.expect("I2S Start");

    let mut index = 1;
    loop {
        if let Err(err) = output_stream.send_from_ram(&buffers[index]).await {
            error!("{}", err);
        }

        index += 1;
        if index >= 3 {
            index = 0;
        }

        waveform.process(&mut buffers[index]);
    }
}

struct Waveform {
    inv_sample_rate: f32,
    carrier: SineOsc,
    freq_mod: SineOsc,
    amp_mod: SineOsc,
}

impl Waveform {
    fn new(inv_sample_rate: f32) -> Self {
        let carrier = SineOsc::new();

        let mut freq_mod = SineOsc::new();
        freq_mod.set_frequency(8.0, inv_sample_rate);
        freq_mod.set_amplitude(1.0);

        let mut amp_mod = SineOsc::new();
        amp_mod.set_frequency(16.0, inv_sample_rate);
        amp_mod.set_amplitude(0.5);

        Self {
            inv_sample_rate,
            carrier,
            freq_mod,
            amp_mod,
        }
    }

    fn process(&mut self, buf: &mut [Sample]) {
        for sample in buf.chunks_mut(1) {
            let freq_modulation = bipolar_to_unipolar(self.freq_mod.generate());
            self.carrier
                .set_frequency(110.0 + 440.0 * freq_modulation, self.inv_sample_rate);

            let amp_modulation = bipolar_to_unipolar(self.amp_mod.generate());
            self.carrier.set_amplitude(amp_modulation);

            let signal = self.carrier.generate();

            sample[0] = (Sample::SCALE as f32 * signal) as Sample;
        }
    }
}

struct SineOsc {
    amplitude: f32,
    modulo: f32,
    phase_inc: f32,
}

impl SineOsc {
    const B: f32 = 4.0 / PI;
    const C: f32 = -4.0 / (PI * PI);
    const P: f32 = 0.225;

    pub fn new() -> Self {
        Self {
            amplitude: 1.0,
            modulo: 0.0,
            phase_inc: 0.0,
        }
    }

    pub fn set_frequency(&mut self, freq: f32, inv_sample_rate: f32) {
        self.phase_inc = freq * inv_sample_rate;
    }

    pub fn set_amplitude(&mut self, amplitude: f32) {
        self.amplitude = amplitude;
    }

    pub fn generate(&mut self) -> f32 {
        let signal = self.parabolic_sin(self.modulo);
        self.modulo += self.phase_inc;
        if self.modulo < 0.0 {
            self.modulo += 1.0;
        } else if self.modulo > 1.0 {
            self.modulo -= 1.0;
        }
        signal * self.amplitude
    }

    fn parabolic_sin(&mut self, modulo: f32) -> f32 {
        let angle = PI - modulo * 2.0 * PI;
        let y = Self::B * angle + Self::C * angle * abs(angle);
        Self::P * (y * abs(y) - y) + y
    }
}

#[inline]
fn abs(value: f32) -> f32 {
    if value < 0.0 {
        -value
    } else {
        value
    }
}

#[inline]
fn bipolar_to_unipolar(value: f32) -> f32 {
    (value + 1.0) / 2.0
}