void AMDemod::update(void)
{
if (!pass) return;
audio_block_t *blocki, *blockq;
int16_t *pi, *pq, *end;
int32_t sum;
blocki = receiveWritable(0); // receives I, end result is written in channel 0
blockq = receiveReadOnly(1); // receives Q
if (!blocki) {
if (blockq) release(blockq);
return;
}
if (!blockq) {
release(blocki);
return;
}
pi = (int16_t *)(blocki->data);
pq = (int16_t *)(blockq->data);
end = pi + AUDIO_BLOCK_SAMPLES;
while (pi < end) {
// square and sum I and Q
sum = *pi * *pi;
sum += *pq * *pq;
// The result of squaring a 1.15 is 2.30.
// Shift the sum up one bit to make it 1.31 (Q31)
// and then that becomes the input to the arm sqrt function
sum <<= 1;
arm_sqrt_q31((q31_t)sum,(q31_t *) &sum);
// The result is in the high order 16 bits of sum
*pi++ = sum >> 16;
pq++;
}
transmit(blocki);
release(blocki);
release(blockq);
}
//int last_idx = 0;
void AudioEffectChorus::update(void)
{
audio_block_t *block;
short *bp;
int sum;
int c_idx;
if(l_delayline == NULL)return;
// do passthru
// It stores the unmodified data in the delay line so that
// it isn't as likely to click
if(num_chorus <= 1) {
// Just passthrough
block = receiveWritable(0);
if(block) {
bp = block->data;
for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
l_circ_idx++;
if(l_circ_idx >= delay_length) {
l_circ_idx = 0;
}
l_delayline[l_circ_idx] = *bp++;
}
transmit(block,0);
release(block);
}
return;
}
// L E F T C H A N N E L
block = receiveWritable(0);
if(block) {
bp = block->data;
uint32_t tmp = delay_length/(num_chorus - 1) - 1;
for(int i = 0;i < AUDIO_BLOCK_SAMPLES;i++) {
l_circ_idx++;
if(l_circ_idx >= delay_length) {
l_circ_idx = 0;
}
l_delayline[l_circ_idx] = *bp;
sum = 0;
c_idx = l_circ_idx;
for(int k = 0; k < num_chorus; k++) {
sum += l_delayline[c_idx];
if(num_chorus > 1)c_idx -= tmp;
if(c_idx < 0) {
c_idx += delay_length;
}
}
*bp++ = sum/num_chorus;
}
// transmit the block
transmit(block,0);
release(block);
}
}
void AudioMixer2::update(void)
{
audio_block_t *in, *out=NULL;
unsigned int channel;
for (channel=0; channel < 2; channel++) {
if (!out) {
out = receiveWritable(channel);
if (out) {
int32_t mult = multiplier[channel];
if (mult != 65536) applyGain(out->data, mult);
}
} else {
in = receiveReadOnly(channel);
if (in) {
applyGainThenAdd(out->data, in->data, multiplier[channel]);
release(in);
}
}
}
if (out) {
transmit(out);
release(out);
}
}
void AudioFilterLBCF::update(void)
{
audio_block_t *block;
int32_t b0, b1, b2, a1, a2, sum;
uint32_t in2, out2, bprev, aprev, cprev, flag;
uint32_t *data, *end;
int32_t *state;
block = receiveWritable();
if (!block) return;
end = (uint32_t *)(block->data) + AUDIO_BLOCK_SAMPLES/2;
state = (int32_t *)definition;
do {
b0 = *state++; // 1
b1 = *state++; // -d
b2 = *state++; // stage
a1 = *state++; // d
a2 = *state++; // f*(1-d)
bprev = *state++;
aprev = *state++;
sum = *state & 0x3FFF;
data = end - AUDIO_BLOCK_SAMPLES/2;
do {
in2 = *data;
circ_idx[b2]++;
if (circ_idx[b2] >= delay_length[b2])
circ_idx[b2] = 0;
cprev = outDelayline[b2][circ_idx[b2]];
sum = signed_multiply_accumulate_32x16b(sum, b0, in2);
sum = signed_multiply_accumulate_32x16t(sum, b1, bprev);
sum = signed_multiply_accumulate_32x16t(sum, a1, aprev);
sum = signed_multiply_accumulate_32x16b(sum, a2, cprev);
out2 = signed_saturate_rshift(sum, 16, 14);
sum &= 0x3FFF;
sum = signed_multiply_accumulate_32x16t(sum, b0, in2);
sum = signed_multiply_accumulate_32x16b(sum, b1, in2);
sum = signed_multiply_accumulate_32x16b(sum, a1, out2);
sum = signed_multiply_accumulate_32x16t(sum, a2, cprev);
bprev = in2;
aprev = pack_16x16(
signed_saturate_rshift(sum, 16, 14), out2);
sum &= 0x3FFF;
bprev = in2;
*data++ = aprev;
outDelayline[b2][circ_idx[b2]] = aprev;
} while (data < end);
flag = *state & 0x80000000;
*state++ = sum | flag;
*(state-2) = aprev;
*(state-3) = bprev;
} while (flag);
transmit(block);
release(block);
}
void AudioEffectCubicDistort::update(void)
{
audio_block_t *audioblock;
int16_t *data, *end;
int32_t calc, temp;
audioblock = receiveWritable(0);
if (!audioblock) return;
data = audioblock->data;
end = audioblock->data + AUDIO_BLOCK_SAMPLES;
do
{
calc = *data;
calc <<= 16;
// Cube
temp = multiply_32x32_rshift32_rounded(calc, calc);
temp = multiply_32x32_rshift32_rounded(calc, temp);
// times 1/3 - peaks at 0.666
//temp = multiply_32x32_rshift32_rounded(calc, 0x2aaaaaaa);
// times 1/5 - peaks at 0.8
temp = multiply_32x32_rshift32_rounded(calc, 0x19999999);
// times 1/6 - peaks at 0.833
//temp = multiply_32x32_rshift32_rounded(calc, 0x15555555);
*data = (calc - temp)>> 16;
data++;
} while (data < end);
transmit(audioblock);
release(audioblock);
}
