74 lines
1.6 KiB
C
74 lines
1.6 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
|
|
|
|
static channel_state_t channels[2];
|
|
static SemaphoreHandle_t mutex;
|
|
|
|
void waveform_init(void)
|
|
{
|
|
mutex = xSemaphoreCreateMutex();
|
|
memset(channels, 0, sizeof(channels));
|
|
channels[0].duty = 0.5f;
|
|
channels[1].duty = 0.5f;
|
|
DBG("waveform_init complete");
|
|
}
|
|
|
|
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;
|
|
channels[ch].phase = 0.0f;
|
|
xSemaphoreGive(mutex);
|
|
DBG("waveform_set ch=%d type=%d freq=%.1f duty=%.2f", ch, type, freq, duty);
|
|
}
|
|
|
|
static inline int16_t generate(channel_state_t *s)
|
|
{
|
|
float p = s->phase;
|
|
float v;
|
|
|
|
switch (s->type) {
|
|
case WAVE_SINE:
|
|
v = sinf(2.0f * M_PI * p);
|
|
break;
|
|
case WAVE_SQUARE:
|
|
v = (p < s->duty) ? 1.0f : -1.0f;
|
|
break;
|
|
case WAVE_SAW:
|
|
if (p < 0.5f)
|
|
v = p * 4.0f - 1.0f;
|
|
else
|
|
v = 3.0f - p * 4.0f;
|
|
break;
|
|
default:
|
|
return 0;
|
|
}
|
|
|
|
s->phase += s->freq / (float)SAMPLE_RATE;
|
|
if (s->phase >= 1.0f)
|
|
s->phase -= 1.0f;
|
|
|
|
return (int16_t)(AMPLITUDE * v);
|
|
}
|
|
|
|
int16_t waveform_next_sample(int ch)
|
|
{
|
|
if (ch < 0 || ch > 1) return 0;
|
|
xSemaphoreTake(mutex, portMAX_DELAY);
|
|
int16_t s = generate(&channels[ch]);
|
|
xSemaphoreGive(mutex);
|
|
return s;
|
|
}
|