fast resonant sine but distorted

resonant.cpp

#include "resonant.h"
#include "utility/dspinst.h"

void AudioSynthWaveformRes::update(void)
{
  audio_block_t *block;
  uint32_t i;

  if (c or s)
  {
    block = allocate();
    if (block)
    {
      for (i = 0; i < AUDIO_BLOCK_SAMPLES; i += 1)
      {
        c = multiply_subtract_32x32_rshift32_rounded(c, a, s);
        s = multiply_accumulate_32x32_rshift32_rounded(s, a, c);
        block->data[i] = s;
      }
      transmit(block);
      release(block);
      return;
    }
  }
  // clean up phase/amplitude drift each block? (though it doesn't drift in my tests)
}

resonant.h

#ifndef resonant_h_
#define resonant_h_

#include "AudioStream.h"
#include "arm_math.h"

class AudioSynthWaveformRes : public AudioStream
{
  public:
    AudioSynthWaveformRes() : AudioStream(0, NULL), c(32767), s(0) {}
    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;
      a = 2 * 2.0 * sin(M_PI * freq / AUDIO_SAMPLE_RATE_EXACT) * ((uint)1 << 31);
      // Why does it need the extra 2 to be correct?  Didn't in Python.
      Serial.println(a);
    }
    void phase(float angle)
    {
      if (angle < 0.0) angle = 0.0;
      else if (angle > 360.0)
      {
        angle = angle - 360.0;
        if (angle >= 360.0) return;
      }
      // Actually the gain needs to be scaled a little because it's not a true circle
      // Out of phase by 90 deg + 1/2 sample?
      c = cos(angle * (M_PI / 180)) * (1 << 30);
      s = sin(angle * (M_PI / 180)) * (1 << 30);
    }
    void amplitude(float n)
    {
      if (n < 0) n = 0;
      else if (n > 1.0) n = 1.0;
      // 16-bit max is -32,768 to +32,767

      // This method is faster, but produces more distortion especially at low levels.
      float mag = sqrt(c * c + s * s);
      mag = (n * 32767) / mag;
      c *= mag;
      s *= mag;
    }
    virtual void update(void);
  private:
    q31_t a; // SMMLSR is for signed multiplicands
    q31_t c = 1 << 30;
    q31_t s = 0;
};

#endif

resonant_sine_magnitude.ino

#include <Audio.h>
#include <Wire.h>
#include <SD.h>
#include <SPI.h>

#include <math.h>
#include "resonant.h"

AudioSynthWaveformRes    waveform1;      //xy=188,240
AudioOutputI2S           i2s1;           //xy=565,241
AudioConnection          patchCord2(waveform1, 0, i2s1, 0);
AudioConnection          patchCord3(waveform1, 0, i2s1, 1);
AudioControlSGTL5000     audioShield;     //xy=586,175

void setup()
{
  Serial.begin(9600);

  AudioMemory(80);
  audioShield.enable();
  audioShield.volume(0.2);

  waveform1.amplitude(0.4);  // 1.26 Vpp for 1.0
  waveform1.frequency(5345.678); // can only go up to 3.5 kHz because of range of a
}

void loop()
{
  Serial.print(AudioProcessorUsage());
  Serial.print(",");
  Serial.println(AudioProcessorUsageMax());
  delay(500);
}