122 lines
3.1 KiB
C
122 lines
3.1 KiB
C
#include "waveform.h"
|
|
#include "debug.h"
|
|
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/semphr.h"
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846f
|
|
#endif
|
|
|
|
#define LUT_SIZE 4096
|
|
#define LUT_SHIFT 20 /* 32 - 12 = 20, maps phase to 12-bit index */
|
|
|
|
#define RING_FRAMES 4800 /* 100ms at 48kHz */
|
|
#define RING_SAMPLES (RING_FRAMES * 2)
|
|
|
|
/* left-justified 24-bit amplitude constants */
|
|
#define AMP_LJ ((int32_t)AMPLITUDE_24BIT << 8)
|
|
#define AMP_LJ_N (-AMP_LJ)
|
|
|
|
static int32_t sine_lut[LUT_SIZE];
|
|
|
|
static channel_state_t channels[2];
|
|
static int32_t ring[RING_SAMPLES];
|
|
static int ring_pos;
|
|
static SemaphoreHandle_t mutex;
|
|
|
|
static int32_t generate_sample(const channel_state_t *ch, uint32_t phase)
|
|
{
|
|
switch (ch->type) {
|
|
case WAVE_SINE:
|
|
return sine_lut[phase >> LUT_SHIFT];
|
|
|
|
case WAVE_SQUARE:
|
|
return (phase < (uint32_t)(ch->duty * 4294967296.0f)) ? AMP_LJ : AMP_LJ_N;
|
|
|
|
case WAVE_SAW: {
|
|
/* symmetric triangle: -amp at 0, +amp at mid, -amp at max */
|
|
int32_t p;
|
|
if (phase < 0x80000000U) {
|
|
/* first half: -amp to +amp */
|
|
p = (int32_t)((int64_t)phase * 2 * AMP_LJ / (int64_t)0x80000000U) - AMP_LJ;
|
|
} else {
|
|
/* second half: +amp to -amp */
|
|
uint32_t ph2 = phase - 0x80000000U;
|
|
p = AMP_LJ - (int32_t)((int64_t)ph2 * 2 * AMP_LJ / (int64_t)0x80000000U);
|
|
}
|
|
return p;
|
|
}
|
|
|
|
default:
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
static void regenerate_ring(void)
|
|
{
|
|
uint32_t phase[2] = {0, 0};
|
|
uint32_t inc[2];
|
|
|
|
for (int c = 0; c < 2; c++) {
|
|
if (channels[c].type != WAVE_NONE && channels[c].freq > 0.0f)
|
|
inc[c] = (uint32_t)((double)channels[c].freq / SAMPLE_RATE * 4294967296.0);
|
|
else
|
|
inc[c] = 0;
|
|
}
|
|
|
|
for (int i = 0; i < RING_FRAMES; i++) {
|
|
ring[i * 2] = generate_sample(&channels[0], phase[0]);
|
|
ring[i * 2 + 1] = generate_sample(&channels[1], phase[1]);
|
|
phase[0] += inc[0];
|
|
phase[1] += inc[1];
|
|
}
|
|
|
|
ring_pos = 0;
|
|
}
|
|
|
|
void waveform_init(void)
|
|
{
|
|
mutex = xSemaphoreCreateMutex();
|
|
memset(channels, 0, sizeof(channels));
|
|
channels[0].duty = 0.5f;
|
|
channels[1].duty = 0.5f;
|
|
|
|
for (int i = 0; i < LUT_SIZE; i++)
|
|
sine_lut[i] = (int32_t)(AMPLITUDE_24BIT * sinf(2.0f * M_PI * i / LUT_SIZE)) << 8;
|
|
|
|
memset(ring, 0, sizeof(ring));
|
|
ring_pos = 0;
|
|
|
|
DBG("waveform_init complete, LUT %d entries", LUT_SIZE);
|
|
}
|
|
|
|
void waveform_set(int ch, wave_type_t type, float freq, float duty)
|
|
{
|
|
if (ch < 0 || ch > 1) return;
|
|
xSemaphoreTake(mutex, portMAX_DELAY);
|
|
channels[ch].type = type;
|
|
channels[ch].freq = freq;
|
|
channels[ch].duty = duty;
|
|
regenerate_ring();
|
|
xSemaphoreGive(mutex);
|
|
DBG("waveform_set ch=%d type=%d freq=%.1f duty=%.2f", ch, type, freq, duty);
|
|
}
|
|
|
|
void waveform_fill_buffer(int32_t *buf, int frames)
|
|
{
|
|
int samples = frames * 2;
|
|
|
|
xSemaphoreTake(mutex, portMAX_DELAY);
|
|
int pos = ring_pos;
|
|
for (int i = 0; i < samples; i++) {
|
|
buf[i] = ring[pos];
|
|
if (++pos >= RING_SAMPLES)
|
|
pos = 0;
|
|
}
|
|
ring_pos = pos;
|
|
xSemaphoreGive(mutex);
|
|
}
|