The Sperry Gyro Autopilot

// Sperry's argument, on a bench: attitude alone is not enough.
//
// One IMU, one servo, one plank on a bearing. The loop reads roll angle, works out how
// far to move the "aileron", and writes it to the servo. Everything about how it behaves
// is in the two gains below.
//
//   KP  acts on the ANGLE       -- how far from level am I?
//   KD  acts on the RATE        -- how fast am I getting worse?
//
// Set KD to 0 and you have a 1912 attitude-only stabiliser. It will not fall over, and it
// will not stop moving either: it wallows, because nothing in the loop takes energy out.
// Wind KD up and the wallow dies. That single constant is the whole lesson.
//
// Wiring: MPU-6050 on I2C (SDA A4, SCL A5, VCC 5V, GND GND). Servo signal on D9, servo
// power from a SEPARATE 5 V supply with its ground tied to the Arduino's -- a servo
// stalling against the plank will brown out the board and reset it mid-experiment.

#include <Wire.h>
#include <Servo.h>

const float KP = 1.8;        // servo degrees per degree of roll error
const float KD = 0.0;        // servo degrees per (degree/second) of roll rate  <-- CHANGE ME
const float SETPOINT = 0.0;  // degrees; wings level

const int   SERVO_PIN   = 9;
const int   SERVO_TRIM  = 90;   // servo angle that leaves the plank level
const float SERVO_LIMIT = 45.0; // do not command past the linkage travel
const unsigned long DT_MS = 10; // 100 Hz loop

const int MPU = 0x68;
Servo aileron;

float angle = 0.0;           // degrees, complementary-filtered
unsigned long tPrev = 0;

void mpuWrite(byte reg, byte val) {
  Wire.beginTransmission(MPU); Wire.write(reg); Wire.write(val); Wire.endTransmission(true);
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpuWrite(0x6B, 0x00);      // wake up
  mpuWrite(0x1C, 0x00);      // accel +/-2 g
  mpuWrite(0x1B, 0x00);      // gyro  +/-250 deg/s
  aileron.attach(SERVO_PIN);
  aileron.write(SERVO_TRIM);
  delay(500);
  Serial.println(F("t_ms,angle_deg,rate_dps,command_deg"));
  tPrev = millis();
}

void loop() {
  unsigned long tNow = millis();
  if (tNow - tPrev < DT_MS) return;
  float dt = (tNow - tPrev) / 1000.0;
  tPrev = tNow;

  // Read accelerometer and gyro in one burst.
  Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true);
  int16_t ax = Wire.read() << 8 | Wire.read();
  int16_t ay = Wire.read() << 8 | Wire.read();
  int16_t az = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();                    // temperature, discarded
  int16_t gx = Wire.read() << 8 | Wire.read();

  float accAngle = atan2((float)ay, (float)az) * 57.2958;   // degrees, noisy but drift-free
  float rate     = (float)gx / 131.0;                       // deg/s, smooth but drifts

  // Complementary filter: trust the gyro over a second, the accelerometer over a minute.
  // This is itself a feedback loop -- the accelerometer slowly corrects the integrated gyro.
  angle = 0.98 * (angle + rate * dt) + 0.02 * accAngle;

  float error   = SETPOINT - angle;
  float command = KP * error - KD * rate;      // rate opposes the motion, hence the minus

  if (command >  SERVO_LIMIT) command =  SERVO_LIMIT;
  if (command < -SERVO_LIMIT) command = -SERVO_LIMIT;
  aileron.write(SERVO_TRIM + (int)command);

  Serial.print(tNow);       Serial.print(',');
  Serial.print(angle, 2);   Serial.print(',');
  Serial.print(rate, 2);    Serial.print(',');
  Serial.println(command, 2);
}