Adafruit BMP280: pressure and altitude on I2C

// 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;