Rocket flight computer on an Arduino Uno

// Model rocket flight computer: altitude, acceleration, a log and a parachute.
//
//   A4, A5       BMP280 at 0x76 and MPU6050 at 0x68, sharing one I2C bus
//   10 11 12 13  microSD module: CS, MOSI, MISO, SCK
//   A0           deploy servo: it holds the parachute door shut until apogee
//   pin 7        active buzzer
//   pin 4        green lamp: ready, and solid while armed
//   pin 5        amber lamp: in flight, flashing once the chute is out
//   pin 6        red lamp: a fault
//   pin 2        ARM button: hold it for a second to arm or disarm
//   pin 3        BENCH button: fly a simulated flight through the same code
//
// # The flight, as a state machine
//
//   IDLE -> ARMED -> BOOST -> COAST -> APOGEE -> DESCENT -> LANDED
//
// Each state looks for exactly one thing. Armed waits for launch, boost waits
// for the motor to burn out, coast waits for the top, and nothing skips ahead.
// That is what keeps a flight computer predictable.
//
// # Launch
//
// The MPU6050 runs on its 16 g range, and launch is three readings in a row
// over 2.5 g. A barometric backup counts too: ten meters above the pad. In
// Mokxi the IMU part reads gravity through its tilt and nothing else, so on
// the canvas it is the pressure slider that flies the rocket.
//
// # Apogee, and why it is filtered
//
// A barometer is noisy, and at the top of a flight the rocket is barely
// moving, so one low reading would look like the way down. The altitude goes
// through an exponential filter, the climb rate is worked out from the
// filtered altitude and filtered again, and apogee needs all three of these:
// a second since launch, the filtered altitude 1.5 m under its highest, and
// the climb rate below zero three samples in a row.
//
// # Whole numbers
//
// An Uno has no floating point hardware, and the SD library alone is a third
// of its flash. So altitude is kept in centimeters, the climb rate in cm/s and
// acceleration in thousandths of a g, all in plain integers.
//
// # Try it
//
// Press Run and watch the self-test. Hold ARM for a second. Then drag the
// BMP280's pressure slider down about 2 hPa (roughly 17 m of climb) and back
// up again. Or press BENCH: a simulated flight, made-up numbers clearly
// marked, flown through exactly the same code. Every sample goes into
// FLIGHT.CSV on the card; click it in the card's file list to download it.
//
// Real rocketry: fly certified motors, follow your national association's
// safety code, and ground test every recovery system before it flies.

#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <Servo.h>
#include <Adafruit_BMP280.h>

const int PIN_ARM = 2;
const int PIN_BENCH = 3;
const int PIN_GREEN = 4;
const int PIN_AMBER = 5;
const int PIN_RED = 6;
const int PIN_BUZZER = 7;
const int PIN_DEPLOY = A0;
const int PIN_CARD = 10;

const uint8_t MPU_ADDRESS = 0x68;
const int LOCKED_DEG = 0;     // the door held shut
const int DEPLOY_DEG = 90;    // the door let go

const unsigned long SAMPLE_MS = 50;       // 20 samples a second
const long LAUNCH_MG = 2500;
const long BACKUP_LAUNCH_CM = 1000;
const long BURNOUT_MG = 1500;
const long APOGEE_DROP_CM = 150;
const unsigned long APOGEE_LOCKOUT_MS = 1000;
const long LANDED_BELOW_CM = 3000;

enum State { IDLE, ARMED, BOOST, COAST, APOGEE, DESCENT, LANDED };
const char *const NAMES[] = {"IDLE", "ARMED", "BOOST", "COAST", "APOGEE", "DESCENT", "LANDED"};

Adafruit_BMP280 baro;
Servo deploy;
File logFile;

bool haveBaro = false;
bool haveImu = false;
bool haveCard = false;

State state = IDLE;
bool bench = false;            // flying the simulated flight
unsigned long stateAt = 0;
unsigned long launchAt = 0;
unsigned long sampleAt = 0;
unsigned long flyAt = 0;           // when fly() last ran, for the climb rate
unsigned long printAt = 0;
unsigned long flushAt = 0;
unsigned long chirpAt = 0;
unsigned long stillSince = 0;
int armHeld = 0;
bool armLatched = false;
bool benchWas = HIGH;

long padCm = 0;                // the pad's altitude above sea level
long altCm = 0;                // filtered height above the pad
long velCms = 0;               // filtered climb rate
long maxCm = 0;
long accelMg = 1000;
long tiltCos = 1000;           // how vertical the board is, 1000 = straight up
int launchCount = 0;
int burnoutCount = 0;
int fallingCount = 0;

// The simulated flight: one second of motor, then gravity, then a chute.
long simCm = 0;
long simCms = 0;
unsigned long simAt = 0;
bool deployed = false;

// --- the buzzer, without blocking ----------------------------------------------

int beepSteps[20];             // ms, alternating on and off
int beepCount = 0;
int beepIndex = 0;
unsigned long beepAt = 0;

void beep(const int *steps, int count) {
  beepCount = count < 20 ? count : 20;
  for (int i = 0; i < beepCount; i++) beepSteps[i] = steps[i];
  beepIndex = 0;
  beepAt = millis();
  digitalWrite(PIN_BUZZER, HIGH);
}

void serviceBuzzer() {
  if (beepIndex >= beepCount) {
    digitalWrite(PIN_BUZZER, LOW);
    return;
  }
  if (millis() - beepAt >= (unsigned long)beepSteps[beepIndex]) {
    beepAt = millis();
    beepIndex++;
    digitalWrite(PIN_BUZZER, (beepIndex < beepCount && beepIndex % 2 == 0) ? HIGH : LOW);
  }
}

// Apogee in beeps, the way a recovery crew reads an altimeter in a field:
// the tens, a pause, the units, and a long beep for a zero.
void beepAltitude(long meters) {
  int steps[20];
  int n = 0;
  int digits[2] = {(int)((meters / 10) % 10), (int)(meters % 10)};
  for (int d = 0; d < 2; d++) {
    if (digits[d] == 0) {
      steps[n++] = 600;
      steps[n++] = 200;
    }
    for (int i = 0; i < digits[d] && n < 16; i++) {
      steps[n++] = 120;
      steps[n++] = 200;
    }
    steps[n - 1] = 800;
  }
  beep(steps, n);
}

// --- printing centimeters as meters ----------------------------------------------

void printMeters(Print &out, long cm) {
  if (cm < 0) {
    out.print('-');
    cm = -cm;
  }
  out.print(cm / 100);
  out.print('.');
  out.print((cm / 10) % 10);
}

// --- sensors ----------------------------------------------------------------------

long baroCm() {
  return (long)(baro.readAltitude(1013.25) * 100);
}

bool imuBegin() {
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x75);                          // WHO_AM_I
  if (Wire.endTransmission(false) != 0) return false;
  Wire.requestFrom(MPU_ADDRESS, (uint8_t)1);
  if (Wire.read() != 0x68) return false;
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x6B);                          // PWR_MGMT_1: wake it up
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x1C);                          // ACCEL_CONFIG: +/-16 g
  Wire.write(0x18);
  Wire.endTransmission();
  return true;
}

// Acceleration in mg, and how vertical the board is (its Z axis runs up the
// airframe): the Z share of the reading, 1000 when it points straight up.
void imuRead(long &mg, long &vertical) {
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x3B);                          // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDRESS, (uint8_t)6);
  long a[3];
  for (int i = 0; i < 3; i++) {
    int16_t raw = (int16_t)(Wire.read() << 8);
    raw |= Wire.read();
    a[i] = (long)raw * 1000 / 2048;          // 2048 counts per g at 16 g
  }
  long squared = a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
  long root = 0;
  while ((root + 1) * (root + 1) <= squared) root += (root < 100) ? 1 : root / 16 + 1;
  while (root * root > squared) root--;
  mg = root;
  vertical = root > 0 ? a[2] * 1000 / root : 0;
}

// One raw sample, from the sensors or from the bench flight.
void readSample(long &rawCm, long &mg) {
  if (bench) {
    long dt = (long)(millis() - simAt);      // ms since the last sample
    simAt = millis();
    long accel = 0;                          // cm/s^2
    if (state >= BOOST && !deployed) {
      accel = (millis() - launchAt < 1000) ? 3000 : -981;
      simCms += accel * dt / 1000;
    }
    if (deployed) simCms = -600;             // six meters a second under the chute
    if (state >= BOOST) simCm += simCms * dt / 1000;
    if (simCm < 0) {
      simCm = 0;
      simCms = 0;
    }
    rawCm = simCm + random(-15, 16);         // a little made-up noise
    mg = (state < BOOST || simCm == 0 || deployed) ? 1000 : (accel + 981) * 1000 / 981;
    return;
  }
  rawCm = haveBaro ? baroCm() - padCm : 0;
  long vertical = 1000;
  mg = 1000;
  if (haveImu) imuRead(mg, vertical);
  tiltCos = vertical;
}

// --- logging -------------------------------------------------------------------------

void logLine(const char *what) {
  Serial.println(what);
  if (!logFile) return;
  logFile.print("# ");
  logFile.println(what);
}

void logSample() {
  if (!logFile) return;
  logFile.print(millis() - launchAt);
  logFile.print(',');
  logFile.print(NAMES[state]);
  logFile.print(',');
  printMeters(logFile, altCm);
  logFile.print(',');
  printMeters(logFile, velCms);
  logFile.print(',');
  logFile.println(accelMg);
}

void enter(State next) {
  state = next;
  stateAt = millis();
  Serial.print("STATE ");
  Serial.println(NAMES[next]);
  if (logFile) {
    logFile.print("# STATE ");
    logFile.println(NAMES[next]);
  }
}

// --- arming --------------------------------------------------------------------------

void zeroPad() {
  long sum = 0;
  for (int i = 0; i < 8; i++) {
    sum += baroCm();
    delay(20);
  }
  padCm = sum / 8;
  altCm = 0;
  velCms = 0;
  maxCm = 0;
}

void arm(bool benchFlight) {
  // cos 30 degrees is 0.866: any further over than that and it will not arm.
  if (!benchFlight && haveImu && tiltCos < 866) {
    Serial.println("NOT ARMED: tilted over 30 deg");
    static const int nope[] = {80, 80, 80, 80, 80};
    beep(nope, 5);
    return;
  }
  bench = benchFlight;
  zeroPad();
  simCm = 0;
  simCms = 0;
  simAt = millis();
  deployed = false;
  deploy.write(LOCKED_DEG);
  if (haveCard) {
    logFile = SD.open("FLIGHT.CSV", FILE_WRITE);
    if (logFile) logFile.println("ms,state,alt_m,vel_ms,accel_mg");
  }
  if (bench) logLine("BENCH FLIGHT (simulated data)");
  launchCount = 0;
  enter(ARMED);
  static const int armed[] = {80, 80, 80, 80, 400};
  beep(armed, 5);
}

void disarm() {
  if (logFile) logFile.close();
  bench = false;
  deploy.write(LOCKED_DEG);
  deployed = false;
  enter(IDLE);
}

// --- the flight ----------------------------------------------------------------------

void fly() {
  long rawCm, mg;
  readSample(rawCm, mg);
  // The bench flight lights its motor half a second after arming.
  if (bench && state == ARMED && millis() - stateAt >= 500) {
    launchAt = millis();
    mg = 4000;
    launchCount = 2;
  }
  accelMg = mg;
  long before = altCm;
  altCm += (rawCm - altCm) * 3 / 10;                   // alpha 0.3
  long dt = (long)(millis() - flyAt);
  flyAt = millis();
  if (dt < 1 || dt > 500) dt = (long)SAMPLE_MS;
  long rate = (altCm - before) * 1000 / dt;
  velCms += (rate - velCms) * 3 / 10;

  switch (state) {
    case ARMED:
      launchCount = (mg > LAUNCH_MG) ? launchCount + 1 : 0;
      if (launchCount >= 3 || altCm > BACKUP_LAUNCH_CM) {
        if (!bench) launchAt = millis();
        logLine(launchCount >= 3 ? "LAUNCH (accel)" : "LAUNCH (baro)");
        enter(BOOST);
        burnoutCount = 0;
      }
      break;
    case BOOST:
      burnoutCount = (mg < BURNOUT_MG) ? burnoutCount + 1 : 0;
      if (burnoutCount >= 3) {
        logLine("BURNOUT");
        enter(COAST);
        fallingCount = 0;
      }
      break;
    case COAST:
      if (altCm > maxCm) maxCm = altCm;
      fallingCount = (velCms < 0) ? fallingCount + 1 : 0;
      if (millis() - launchAt > APOGEE_LOCKOUT_MS && altCm < maxCm - APOGEE_DROP_CM &&
          fallingCount >= 3) {
        enter(APOGEE);
        deploy.write(DEPLOY_DEG);
        deployed = true;
        Serial.print("APOGEE ");
        printMeters(Serial, maxCm);
        Serial.println(" m: DEPLOY");
        logLine("DEPLOY");
        static const int fired[] = {300};
        beep(fired, 1);
        enter(DESCENT);
        stillSince = millis();
      }
      break;
    case DESCENT:
      // Landed: still for three seconds, and near the height of the pad.
      if (velCms > 100 || velCms < -100 || altCm > LANDED_BELOW_CM) stillSince = millis();
      if (millis() - stillSince > 3000) {
        enter(LANDED);
        Serial.print("FLIGHT OVER: apogee ");
        printMeters(Serial, maxCm);
        Serial.print(" m in ");
        Serial.print((millis() - launchAt) / 1000);
        Serial.println(" s");
        if (logFile) {
          logFile.print("# APOGEE ");
          printMeters(logFile, maxCm);
          logFile.println();
          logFile.close();
          Serial.println("FLIGHT.CSV saved");
        }
        beepAltitude((maxCm + 50) / 100);
      }
      break;
    default:
      break;
  }
  if (state >= BOOST && state <= DESCENT) logSample();
}

// --- lamps ---------------------------------------------------------------------------

void showLamps() {
  unsigned long now = millis();
  bool slow = (now / 500) % 2;
  bool fast = (now / 125) % 2;
  digitalWrite(PIN_RED, haveBaro ? LOW : HIGH);
  digitalWrite(PIN_GREEN,
               state == ARMED || (state == IDLE && haveBaro && slow) || (state == LANDED && slow));
  digitalWrite(PIN_AMBER,
               state == BOOST || state == COAST || (state == DESCENT && fast) || (state == LANDED && !slow));
}

// --- setup and loop ------------------------------------------------------------------

void selfTest() {
  Serial.println("SELF-TEST");
  haveBaro = baro.begin(0x76);
  if (haveBaro) {
    baro.setSampling(Adafruit_BMP280::MODE_NORMAL, Adafruit_BMP280::SAMPLING_X2,
                     Adafruit_BMP280::SAMPLING_X16, Adafruit_BMP280::FILTER_OFF,
                     Adafruit_BMP280::STANDBY_MS_1);
    Serial.print("  baro OK ");
    printMeters(Serial, baroCm());
    Serial.println(" m ASL");
  } else {
    Serial.println("  baro FAIL: no BMP280");
  }
  haveImu = imuBegin();
  if (haveImu) {
    long mg, vertical;
    imuRead(mg, vertical);
    tiltCos = vertical;
    Serial.print("  imu OK ");
    Serial.print(mg);
    Serial.println(" mg");
  } else {
    Serial.println("  imu WARN: no MPU6050");
  }
  haveCard = SD.begin(PIN_CARD);
  Serial.println(haveCard ? "  card OK" : "  card WARN: no log");
  deploy.attach(PIN_DEPLOY);
  deploy.write(LOCKED_DEG);
  Serial.println("  deploy LOCKED");
  if (haveBaro) {
    Serial.println("SELF-TEST PASS");
    static const int ok[] = {100, 100, 100, 100, 100};
    static const int warn[] = {100, 100, 100, 100, 100, 100, 600};
    if (haveImu && haveCard) beep(ok, 5);
    else beep(warn, 7);
  } else {
    Serial.println("SELF-TEST FAIL");
    static const int bad[] = {1500};
    beep(bad, 1);
  }
}

void setup() {
  pinMode(PIN_ARM, INPUT_PULLUP);
  pinMode(PIN_BENCH, INPUT_PULLUP);
  pinMode(PIN_RED, OUTPUT);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_AMBER, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  Serial.begin(115200);
  Serial.println("Mokxi flight computer");
  Wire.begin();
  selfTest();
  if (haveBaro) zeroPad();
  enter(IDLE);
}

void handleButtons() {
  // ARM is a hold, not a tap, so a bump on the pad cannot arm it.
  if (digitalRead(PIN_ARM) == LOW) {
    armHeld++;
  } else {
    armHeld = 0;
    armLatched = false;
  }
  if (armHeld >= (int)(1000 / SAMPLE_MS) && !armLatched) {
    armLatched = true;
    if (state == IDLE && haveBaro) arm(false);
    else if (state == ARMED || state == LANDED) disarm();
  }
  bool benchNow = digitalRead(PIN_BENCH);
  if (benchWas == HIGH && benchNow == LOW && state == IDLE && haveBaro) arm(true);
  benchWas = benchNow;
}

void loop() {
  serviceBuzzer();
  deploy.refresh();
  showLamps();
  if (millis() - sampleAt < SAMPLE_MS) return;
  sampleAt = millis();

  handleButtons();
  if (state == IDLE) {
    long rawCm, mg;
    readSample(rawCm, mg);
    altCm += (rawCm - altCm) * 3 / 10;
    accelMg = mg;
  } else if (state != LANDED) {
    fly();
  }

  // The armed chirp, every two seconds, so nobody walks up to a live rocket.
  if (state == ARMED && millis() - chirpAt >= 2000 && millis() - stateAt > 1000) {
    chirpAt = millis();
    static const int chirp[] = {60};
    beep(chirp, 1);
  }
  if (state == LANDED && millis() - stateAt > 12000) {
    stateAt = millis();
    beepAltitude((maxCm + 50) / 100);
  }
  if (logFile && millis() - flushAt > 1000) {
    flushAt = millis();
    logFile.flush();
  }

  unsigned long every = (state >= BOOST && state <= DESCENT) ? 250 : 1000;
  if (millis() - printAt >= every) {
    printAt = millis();
    Serial.print(NAMES[state]);
    Serial.print("  alt ");
    printMeters(Serial, altCm);
    Serial.print(" m  vel ");
    printMeters(Serial, velCms);
    Serial.print(" m/s  acc ");
    Serial.print(accelMg);
    Serial.println(" mg");
  }
}