Fix AudioSynthWaveform (had many bugs, ugh....)

synth_waveform.cpp

@@ -40,127 +40,122 @@
 void AudioSynthWaveform::update(void)
 {
 	audio_block_t *block;
-	short *bp, *end;
-	int32_t val1, val2, val3;
-	uint32_t index, scale;
-	uint32_t mag;
-	short tmp_amp;
-
-	if (tone_amp == 0) return;
+	int16_t *bp, *end;
+	int32_t val1, val2;
+	int16_t magnitude15;
+	uint32_t i, ph, index, index2, scale;
+	const uint32_t inc = phase_increment;
+
+	ph = phase_accumulator + phase_offset;
+	if (magnitude == 0) {
+		phase_accumulator += inc * AUDIO_BLOCK_SAMPLES;
+		return;
+	}
 	block = allocate();
 	if (!block) {
+		phase_accumulator += inc * AUDIO_BLOCK_SAMPLES;
 		return;
 	}
 	bp = block->data;
+
 	switch(tone_type) {
 	case WAVEFORM_SINE:
-		for(int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			// Calculate interpolated sin
-			index = tone_phase >> 23;
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+			index = ph >> 24;
 			val1 = AudioWaveformSine[index];
 			val2 = AudioWaveformSine[index+1];
-			scale = (tone_phase >> 7) & 0xFFFF;
+			scale = (ph >> 8) & 0xFFFF;
 			val2 *= scale;
-			val1 *= 0xFFFF - scale;
-			val3 = (val1 + val2) >> 16;
-			*bp++ = (short)((val3 * tone_amp) >> 15);
-
-			// phase and incr are both unsigned 32-bit fractions
-			tone_phase += tone_incr;
-			// If tone_phase has overflowed, truncate the top bit
-			if (tone_phase & 0x80000000) tone_phase &= 0x7fffffff;
+			val1 *= 0x10000 - scale;
+			*bp++ = multiply_32x32_rshift32(val1 + val2, magnitude);
+			ph += inc;
 		}
 		break;
 
 	case WAVEFORM_ARBITRARY:
 		if (!arbdata) {
 			release(block);
+			phase_accumulator += inc * AUDIO_BLOCK_SAMPLES;
 			return;
 		}
 		// len = 256
-		for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			index = tone_phase >> 23;
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+			index = ph >> 24;
+			index2 = index + 1;
+			if (index2 >= 256) index2 = 0;
 			val1 = *(arbdata + index);
-			val2 = *(arbdata + ((index + 1) & 255));
-			scale = (tone_phase >> 7) & 0xFFFF;
+			val2 = *(arbdata + index2);
+			scale = (ph >> 8) & 0xFFFF;
 			val2 *= scale;
-			val1 *= 0xFFFF - scale;
-			val3 = (val1 + val2) >> 16;
-			*bp++ = (short)((val3 * tone_amp) >> 15);
-			tone_phase += tone_incr;
-			tone_phase &= 0x7fffffff;
+			val1 *= 0x10000 - scale;
+			*bp++ = multiply_32x32_rshift32(val1 + val2, magnitude);
+			ph += inc;
 		}
 		break;
 
 	case WAVEFORM_SQUARE:
-		for(int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			if (tone_phase & 0x40000000) {
-				*bp++ = -tone_amp;
+		magnitude15 = signed_saturate_rshift(magnitude, 16, 1);
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+			if (ph & 0x80000000) {
+				*bp++ = -magnitude15;
 			} else {
-				*bp++ = tone_amp;
+				*bp++ = magnitude15;
 			}
-			// phase and incr are both unsigned 32-bit fractions
-			tone_phase += tone_incr;
+			ph += inc;
 		}
 		break;
 
 	case WAVEFORM_SAWTOOTH:
-		for(int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			*bp++ = ((short)(tone_phase>>15) * tone_amp) >> 15;
-			// phase and incr are both unsigned 32-bit fractions
-			tone_phase += tone_incr;
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+			*bp++ = signed_multiply_32x16t(magnitude, ph);
+			ph += inc;
 		}
 		break;
 
 	case WAVEFORM_SAWTOOTH_REVERSE:
-		for(int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			*bp++ = ((short)(tone_phase>>15) * tone_amp) >> 15;
-			// phase and incr are both unsigned 32-bit fractions
-			tone_phase -= tone_incr;
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+			*bp++ = signed_multiply_32x16t(0xFFFFFFFFu - magnitude, ph);
+			ph += inc;
 		}
 		break;
 
 	case WAVEFORM_TRIANGLE:
-		for(int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			if (tone_phase & 0x80000000) {
-				// negative half-cycle
-				tmp_amp = -tone_amp;
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+			uint32_t phtop = ph >> 30;
+			if (phtop == 1 || phtop == 2) {
+				*bp++ = ((0x10000 - (ph >> 15)) * magnitude) >> 16;
 			} else {
-				// positive half-cycle
-				tmp_amp = tone_amp;
+				*bp++ = ((ph >> 15) * magnitude) >> 16;
 			}
-			mag = tone_phase << 2;
-			// Determine which quadrant
-			if (tone_phase & 0x40000000) {
-				// negate the magnitude
-				mag = ~mag + 1;
-			}
-			*bp++ = ((short)(mag>>17) * tmp_amp) >> 15;
-			tone_phase += tone_incr * 2;
+			ph += inc;
 		}
 		break;
 
 	case WAVEFORM_PULSE:
-		for(int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			if (tone_phase < tone_width) {
-				*bp++ = -tone_amp;
+		magnitude15 = signed_saturate_rshift(magnitude, 16, 1);
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
+			if (ph < pulse_width) {
+				*bp++ = magnitude15;
 			} else {
-				*bp++ = tone_amp;
+				*bp++ = -magnitude15;
 			}
-			tone_phase += tone_incr;
+			ph += inc;
 		}
 		break;
 
 	case WAVEFORM_SAMPLE_HOLD:
-		for(int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
-			if (tone_phase < tone_incr) {
-				sample = random(-tone_amp, tone_amp);
-			}
+		for (i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
 			*bp++ = sample;
-			tone_phase += tone_incr;
+			uint32_t newph = ph + inc;
+			if (newph < ph) {
+				sample = random(magnitude) - (magnitude >> 1);
+			}
+			ph = newph;
 		}
 		break;
 	}
+	phase_accumulator = ph - phase_offset;
+
 	if (tone_offset) {
 		bp = block->data;
 		end = bp + AUDIO_BLOCK_SAMPLES;

synth_waveform.h

@@ -50,18 +50,21 @@ extern const int16_t AudioWaveformSine[257];
 class AudioSynthWaveform : public AudioStream
 {
 public:
-	AudioSynthWaveform(void) : AudioStream(0,NULL), tone_amp(0), tone_freq(0),
-		tone_phase(0), tone_width(0.25), tone_incr(0), tone_type(0),
-		tone_offset(0), arbdata(NULL) {
+	AudioSynthWaveform(void) : AudioStream(0,NULL),
+		phase_accumulator(0), phase_increment(0), phase_offset(0),
+		magnitude(0), pulse_width(0x40000000),
+		arbdata(NULL), sample(0), tone_type(WAVEFORM_SINE),
+		tone_offset(0) {
 	}
 
-	void frequency(float t_freq) {
-		if (t_freq < 0.0) {
-			t_freq = 0.0;
-		} else if (t_freq > AUDIO_SAMPLE_RATE_EXACT / 2) {
-			t_freq = AUDIO_SAMPLE_RATE_EXACT / 2;
+	void frequency(float freq) {
+		if (freq < 0.0) {
+			freq = 0.0;
+		} else if (freq > AUDIO_SAMPLE_RATE_EXACT / 2) {
+			freq = AUDIO_SAMPLE_RATE_EXACT / 2;
 		}
-		tone_incr = (t_freq * (0x80000000LL/AUDIO_SAMPLE_RATE_EXACT)) + 0.5;
+		phase_increment = freq * (4294967296.0 / AUDIO_SAMPLE_RATE_EXACT);
+		//tone_incr = (t_freq * (0x80000000LL/AUDIO_SAMPLE_RATE_EXACT)) + 0.5;
 	}
 	void phase(float angle) {
 		if (angle < 0.0) {
@@ -70,7 +73,7 @@ public:
 			angle = angle - 360.0;
 			if (angle >= 360.0) return;
 		}
-		tone_phase = angle * (2147483648.0 / 360.0);
+		phase_offset = angle * (4294967296.0 / 360.0);
 	}
 	void amplitude(float n) {	// 0 to 1.0
 		if (n < 0) {
@@ -78,13 +81,7 @@ public:
 		} else if (n > 1.0) {
 			n = 1.0;
 		}
-		if ((tone_amp == 0) && n) {
-			// reset the phase when the amplitude was zero
-			// and has now been increased.
-			tone_phase = 0;
-		}
-		// set new magnitude
-		tone_amp = n * 32767.0;
+		magnitude = n * 65536.0;
 	}
 	void offset(float n) {
 		if (n < -1.0) {
@@ -100,17 +97,17 @@ public:
 		} else if (n > 1.0) {
 			n = 1.0;
 		}
-		tone_width = n * 0x7fffffffLL;
-		// pulse width is stored as the equivalent phase
+		pulse_width = n * 4294967296.0;
 	}
 	void begin(short t_type) {
-		tone_phase = 0;
+		phase_offset = 0;
 		tone_type = t_type;
 	}
 	void begin(float t_amp, float t_freq, short t_type) {
 		amplitude(t_amp);
 		frequency(t_freq);
-		begin(t_type);
+		phase_offset = 0;
+		tone_type = t_type;
 	}
 	void arbitraryWaveform(const int16_t *data, float maxFreq) {
 		arbdata = data;
@@ -118,17 +115,15 @@ public:
 	virtual void update(void);
 
 private:
-	short    tone_amp;
-	short    tone_freq;
-	uint32_t tone_phase;
-	uint32_t tone_width;
-	// sample for SAMPLE_HOLD
-	short sample;
-	// volatile prevents the compiler optimizing out the frequency function
-	volatile uint32_t tone_incr;
+	uint32_t phase_accumulator;
+	uint32_t phase_increment;
+	uint32_t phase_offset;
+	int32_t  magnitude;
+	uint32_t pulse_width;
+	const int16_t *arbdata;
+	int16_t  sample; // for WAVEFORM_SAMPLE_HOLD
 	short    tone_type;
 	int16_t  tone_offset;
-	const int16_t *arbdata;
 };