Spaceship control panel on an Arduino Mega

// Spaceship control panel: launch control for a two-stage rocket, on a Mega.
//
// Work the pre-flight checklist, pass the go/no-go poll, lift the MASTER ARM
// safety and press LAUNCH. Then fly: push the throttle up for liftoff, keep the
// attitude on the guidance line with the stick, press STAGE when the first
// stage runs dry, and fix whatever the master alarm throws at you on the way
// to orbit.
//
//   Arming       three toggles (AVIONICS, TANK PRESS, ENGINE ARM) on 49, 51
//                and 53, each confirmed with its lit arcade button (9, 7, 5;
//                lamps on 8, 6, 4)
//   MASTER ARM   the safety switch on 3, beside the LAUNCH button
//   LAUNCH       green arcade button on 14, its lamp on 15 (it is STAGE too)
//   MASTER ALARM red arcade button on 16, its lamp on 17
//   Throttle     slide pot on A0
//   Attitude     joystick on A1 (yaw) and A2 (pitch)
//   Mission      LCD2004 on I2C (SDA 20, SCL 21), address 0x27
//   Clock        TM1637 on 12 (CLK) and 11 (DIO): countdown, then mission time
//   Fuel         ten-bar LED graph on the odd pins 29 to 47, one bar a pin
//   Engine       16 NeoPixel ring on 2
//   Pitch gauge  servo on 18: the needle stands up at 90 degrees of pitch and
//                lies down as the rocket turns toward the horizon
//   Sound        piezo on 10
//
// The physics are real formulas with made-up rocket numbers: thrust that grows
// as the tanks empty, gravity that weakens with height, air that thins every
// 8.5 km, and an orbit judged by its perigee, the lowest point of the path. The
// flight runs six times faster than real time, so the eight and a half minutes
// a real climb to orbit takes pass in under a minute and a half. The mission
// clock on the TM1637 shows the flight's own time.

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <TM1637Display.h>
#include <Adafruit_NeoPixel.h>
#include <Servo.h>

// ---- pins ------------------------------------------------------------------

const int TM_CLK = 12;
const int TM_DIO = 11;
const int PIEZO = 10;
const int MASTER_ARM = 3;
const int RING_PIN = 2;
const int LAUNCH_BUTTON = 14;
const int LAUNCH_LAMP = 15;
const int ALARM_BUTTON = 16;
const int ALARM_LAMP = 17;
const int GAUGE_PIN = 18;

const int ARM_SWITCH[3] = {49, 51, 53};
const int ARM_BUTTON[3] = {9, 7, 5};
const int ARM_LAMP[3] = {8, 6, 4};

const int BAR_PIN[10] = {29, 31, 33, 35, 37, 39, 41, 43, 45, 47};

const int THROTTLE = A0;
const int STICK_X = A1;
const int STICK_Y = A2;

const char *const SYSTEM[3] = {"AVIONICS", "TANK PRESS", "ENGINE ARM"};

LiquidCrystal_I2C lcd(0x27, 20, 4);
TM1637Display clockDisplay(TM_CLK, TM_DIO);
Adafruit_NeoPixel ring(16, RING_PIN, NEO_GRB + NEO_KHZ800);
Servo gauge;

// ---- the panel's inputs ------------------------------------------------------

// A switch to ground with the pull-up on: LOW is on, or pressed.
bool isOn(int pin) { return digitalRead(pin) == LOW; }

struct Button {
  int pin;
  bool down;
  unsigned long changed;
};

Button launchButton = {LAUNCH_BUTTON, false, 0};
Button alarmButton = {ALARM_BUTTON, false, 0};
Button armButton[3] = {{9, false, 0}, {7, false, 0}, {5, false, 0}};

// True once per press, after 20 ms of the contacts agreeing.
bool pressed(Button &b) {
  bool now = isOn(b.pin);
  if (now != b.down && millis() - b.changed > 20) {
    b.down = now;
    b.changed = millis();
    return now;
  }
  return false;
}

int throttlePercent() {
  long raw = analogRead(THROTTLE);
  int pct = (int)((raw * 100 + 511) / 1023);
  return constrain(pct, 0, 100);
}

// The stick, -1.0 to 1.0, with a dead zone in the middle.
float stick(int pin) {
  int offset = analogRead(pin) - 512;
  if (offset > -60 && offset < 60) return 0.0;
  return constrain(offset / 450.0, -1.0, 1.0);
}

// ---- displays and sound ------------------------------------------------------

// A row of text built a piece at a time: words with s(), numbers with d(),
// right-aligned in `width` characters, with a + on positive numbers when
// `sign` is set and `fill` in front.
struct Row {
  char t[24];
  uint8_t n = 0;
  Row() { t[0] = 0; }
  Row &s(const char *x) {
    while (*x && n < 22) t[n++] = *x++;
    t[n] = 0;
    return *this;
  }
  Row &d(long v, int width = 0, bool sign = false, char fill = ' ') {
    char b[12];
    int k = 0;
    bool neg = v < 0;
    unsigned long u = neg ? -v : v;
    do {
      b[k++] = '0' + u % 10;
      u /= 10;
    } while (u);
    if (neg) b[k++] = '-';
    else if (sign) b[k++] = '+';
    while (k < width) b[k++] = fill;
    while (k) {
      char c = b[--k];
      if (n < 22) t[n++] = c;
    }
    t[n] = 0;
    return *this;
  }
};

// What each row should say, and what the glass says now. An I2C backpack
// takes a couple of milliseconds a character, so a whole screen at once
// would make the panel deaf to its buttons for a fifth of a second. Instead
// showLine() only records the text, and serviceLcd() sends one row's changed
// characters each time round loop().
char lines[4][21];
char shown[4][21];
int nextRow = 0;

void showLine(int row, const char *text) {
  int i = 0;
  for (; i < 20 && text[i]; i++) lines[row][i] = text[i];
  for (; i < 20; i++) lines[row][i] = ' ';
  lines[row][20] = 0;
}

void serviceLcd() {
  for (int tries = 0; tries < 4; tries++) {
    int row = nextRow;
    nextRow = (nextRow + 1) % 4;
    if (strcmp(lines[row], shown[row]) == 0) continue;
    int col = 0;
    while (col < 20) {
      if (lines[row][col] == shown[row][col]) {
        col++;
        continue;
      }
      lcd.setCursor(col, row);
      while (col < 20 && lines[row][col] != shown[row][col]) {
        lcd.write(lines[row][col]);
        shown[row][col] = lines[row][col];
        col++;
      }
    }
    return;
  }
}

void beep(int hz, int ms) { tone(PIEZO, hz, ms); }

const uint8_t MINUS = 0x40;

// T-10 to T-0 as "- 10" on the four digits.
void showCountdown(int seconds) {
  uint8_t digits[4] = {MINUS, 0, 0, 0};
  digits[2] = seconds >= 10 ? clockDisplay.encodeDigit(seconds / 10) : 0;
  digits[3] = clockDisplay.encodeDigit(seconds % 10);
  clockDisplay.setSegments(digits);
}

// Mission elapsed time as mm:ss with the colon lit.
void showMissionTime(float seconds) {
  long s = (long)seconds;
  int shown = (int)((s / 60) % 100) * 100 + (int)(s % 60);
  clockDisplay.showNumberDecEx(shown, 0x40, true);
}

void showFuel(float fraction, bool blinkLow) {
  int bars = (int)(fraction * 10.0 + 0.99);
  if (fraction <= 0.0) bars = 0;
  bool lowBlink = blinkLow && fraction > 0.0 && fraction < 0.1 && (millis() / 250) % 2 == 0;
  for (int i = 0; i < 10; i++) {
    bool lit = i < bars;
    if (i == 0 && lowBlink) lit = false;
    digitalWrite(BAR_PIN[i], lit ? HIGH : LOW);
  }
}

// The needle stands straight up at 90 degrees of pitch and lies flat at 0.
int lastGauge = -1;
void showPitch(float pitch) {
  int angle = constrain((int)(pitch * 2.0 + 0.5), 0, 180);
  if (angle != lastGauge) {
    gauge.write(angle);
    lastGauge = angle;
  }
}

// Engine glow: orange for the first stage, blue-white for the second, a
// flicker on top that grows with the throttle.
void showEngine(int stageNumber, float power) {
  for (int i = 0; i < 16; i++) {
    int flicker = random(60, 101);
    int level = (int)(power * flicker * 2.55);
    level = constrain(level, 0, 255);
    if (stageNumber == 1) {
      ring.setPixelColor(i, level, level * 2 / 5, level / 12);
    } else {
      ring.setPixelColor(i, level * 3 / 5, level * 3 / 4, level);
    }
  }
  ring.show();
}

void ringOff() {
  ring.clear();
  ring.show();
}

// In orbit the ring turns slowly, one dim blue light going round.
void showOrbitRing() {
  int lead = (millis() / 120) % 16;
  for (int i = 0; i < 16; i++) {
    int d = (lead - i + 16) % 16;
    int level = d == 0 ? 90 : d == 1 ? 35 : d == 2 ? 10 : 0;
    ring.setPixelColor(i, 0, level / 3, level);
  }
  ring.show();
}

// ---- the mission -------------------------------------------------------------

enum Phase { CHECKLIST, POLL, READY, COUNTDOWN, HOLDDOWN, FLIGHT, STAGING, ORBIT, ABORTED };
Phase phase = CHECKLIST;
unsigned long phaseStarted = 0;

bool confirmed[3] = {false, false, false};
int pollStep = 0;
int countdown = 10;

// Flight state, in flight seconds (six to every real one), meters and m/s.
const float SPEEDUP = 6.0;
const float EARTH_R = 6371000.0;
const float MU = 3.986e14;
const float TARGET_PERIGEE = 160000.0;

float flightTime = 0;
float altitude = 0, vUp = 0, vAcross = 0;
float fuel = 1.0;
int stageNumber = 1;
float attitudeError = 0;  // degrees of pitch off the guidance line
float yawError = 0;       // degrees off the plane of the orbit
float maxQ = 0;
bool maxQCalled = false;
bool towerCalled = false;
bool throttleUpCalled = false;
bool fairingCalled = false;
float steeringSum = 0;
float steeringTime = 0;

// Faults: one in each stage, at a random moment, of a random kind.
enum Fault { NO_FAULT, GUIDANCE_FAULT, TANK_FAULT, OVERTEMP_FAULT };
Fault fault = NO_FAULT;
bool alarmSilenced = false;
float faultAt[2] = {0, 0};
Fault faultKind[2] = {NO_FAULT, NO_FAULT};
int faultsFired = 0;
unsigned long faultStarted = 0;
unsigned long coolSince = 0;
bool switchWasOn[3] = {true, true, true};
bool switchCycled[3] = {false, false, false};
const unsigned long FAULT_LIMIT_MS = 25000;
const char *abortReason = "";

unsigned long messageUntil = 0;
char message[21] = "";

void callout(const char *text, bool print = true) {
  strncpy(message, text, 20);
  message[20] = 0;
  messageUntil = millis() + 2500;
  if (print) Serial.println(text);
}

void enter(Phase next) {
  phase = next;
  phaseStarted = millis();
}

void setLamp(int pin, bool on) { digitalWrite(pin, on ? HIGH : LOW); }
bool blinkOn(int periodMs) { return (millis() / (unsigned long)(periodMs / 2)) % 2 == 0; }

float guidancePitch() {
  if (altitude < 1500.0) return 90.0;
  float x = (altitude - 1500.0) / 150000.0;
  if (x > 1.0) x = 1.0;
  return 90.0 - 85.0 * pow(x, 0.55);
}

float speed() { return sqrt(vUp * vUp + vAcross * vAcross); }

// The lowest and highest points of the orbit this speed and height would
// give, in meters above the ground, from the energy and the angular momentum
// of the path. The lowest point, the perigee, is what decides whether it is
// an orbit at all: below about 150 km the air drags it down within days.
float perigee = 0;
float apogee = 0;

void workOutOrbit() {
  float r = EARTH_R + altitude;
  float energy = (vUp * vUp + vAcross * vAcross) / 2.0 - MU / r;
  if (energy >= 0) {
    perigee = apogee = 1e9;
    return;
  }
  float a = -MU / (2.0 * energy);
  float h = r * vAcross;
  float e = sqrt(max(0.0, 1.0 + 2.0 * energy * h * h / (MU * MU)));
  perigee = a * (1.0 - e) - EARTH_R;
  apogee = a * (1.0 + e) - EARTH_R;
}

void resetFlight() {
  flightTime = 0;
  altitude = 0;
  vUp = 0;
  vAcross = 0;
  fuel = 1.0;
  stageNumber = 1;
  attitudeError = 0;
  yawError = 0;
  maxQ = 0;
  maxQCalled = towerCalled = throttleUpCalled = fairingCalled = false;
  steeringSum = steeringTime = 0;
  fault = NO_FAULT;
  alarmSilenced = false;
  faultsFired = 0;
  faultAt[0] = random(45, 120);
  faultAt[1] = random(200, 330);
  faultKind[0] = (Fault)random(1, 4);
  faultKind[1] = (Fault)random(1, 4);
  while (faultKind[1] == faultKind[0]) faultKind[1] = (Fault)random(1, 4);
}

// ---- faults ----------------------------------------------------------------

const char *faultName(Fault f) {
  switch (f) {
    case GUIDANCE_FAULT: return "GUIDANCE FAULT";
    case TANK_FAULT: return "TANK PRESS LOW";
    case OVERTEMP_FAULT: return "ENG OVERTEMP";
    default: return "";
  }
}

const char *faultFix(Fault f) {
  switch (f) {
    case GUIDANCE_FAULT: return "CYCLE AVIONICS";
    case TANK_FAULT: return "CYCLE TANK PRESS";
    case OVERTEMP_FAULT: return "THROTTLE BELOW 70%";
    default: return "";
  }
}

void raiseFault(Fault f) {
  fault = f;
  alarmSilenced = false;
  faultStarted = millis();
  coolSince = 0;
  for (int i = 0; i < 3; i++) {
    switchWasOn[i] = isOn(ARM_SWITCH[i]);
    switchCycled[i] = false;
  }
  Serial.print("MASTER ALARM: ");
  Serial.println(faultName(f));
}

void clearFault() {
  Serial.print("FAULT CLEARED: ");
  Serial.println(faultName(fault));
  fault = NO_FAULT;
  setLamp(ALARM_LAMP, false);
  noTone(PIEZO);
  callout("FAULT CLEARED", false);
  beep(1760, 120);
}

// Watch for the fix. A switch counts once it has gone off and come back on.
void checkFault(int throttle) {
  if (fault == NO_FAULT) return;
  for (int i = 0; i < 3; i++) {
    bool on = isOn(ARM_SWITCH[i]);
    if (!on && switchWasOn[i]) {
      switchCycled[i] = true;
    }
    if (on && !switchWasOn[i] && switchCycled[i]) {
      bool right = (fault == GUIDANCE_FAULT && i == 0) || (fault == TANK_FAULT && i == 1);
      if (right) {
        clearFault();
        return;
      }
      callout("NOT THAT SWITCH");
      switchCycled[i] = false;
    }
    switchWasOn[i] = on;
  }
  if (fault == OVERTEMP_FAULT) {
    if (throttle < 70) {
      if (coolSince == 0) coolSince = millis();
      if (millis() - coolSince > 2500) {
        clearFault();
        return;
      }
    } else {
      coolSince = 0;
    }
  }
  if (pressed(alarmButton) && !alarmSilenced) {
    alarmSilenced = true;
    noTone(PIEZO);
    Serial.println("MASTER ALARM SILENCED");
  }
  if (!alarmSilenced) {
    setLamp(ALARM_LAMP, blinkOn(400));
    tone(PIEZO, blinkOn(400) ? 1000 : 1500);
  } else {
    setLamp(ALARM_LAMP, true);
  }
  if (millis() - faultStarted > FAULT_LIMIT_MS) {
    abortReason = faultName(fault);
    noTone(PIEZO);
    setLamp(ALARM_LAMP, true);
    Serial.print("ABORT: ");
    Serial.println(abortReason);
    enter(ABORTED);
  }
}

// ---- the phases ----------------------------------------------------------------

void runChecklist() {
  bool allArmed = true;
  for (int i = 0; i < 3; i++) {
    bool on = isOn(ARM_SWITCH[i]);
    bool push = pressed(armButton[i]);
    if (!on) {
      if (confirmed[i]) {
        Serial.print(SYSTEM[i]);
        Serial.println(" SAFE");
      }
      confirmed[i] = false;
      setLamp(ARM_LAMP[i], false);
    } else if (!confirmed[i]) {
      setLamp(ARM_LAMP[i], blinkOn(500));
      if (push) {
        confirmed[i] = true;
        setLamp(ARM_LAMP[i], true);
        Serial.print(SYSTEM[i]);
        Serial.println(" ARMED");
        beep(1320, 60);
      }
    }
    if (!confirmed[i]) allArmed = false;
  }
  int throttle = throttlePercent();
  bool throttleMin = throttle <= 5;
  bool masterSafe = !isOn(MASTER_ARM);

  showLine(0, "PRE-FLIGHT CHECKLIST");
  showLine(1, Row().s("AVIONICS ").s(confirmed[0] ? "GO" : "--").s(" TANK ").s(confirmed[1] ? "GO" : "--").t);
  showLine(2, Row().s("ENG ARM ").s(confirmed[2] ? "GO" : "--").s(" THR ").s(throttleMin ? "MIN" : "HI").t);
  if (!masterSafe) {
    showLine(3, "SAFE THE MASTER ARM");
  } else if (!allArmed) {
    showLine(3, "FLIP, THEN PRESS LIT");
  } else if (!throttleMin) {
    showLine(3, "THROTTLE TO MINIMUM");
  } else {
    showLine(3, "CHECKLIST COMPLETE");
    Serial.println("CHECKLIST COMPLETE");
    pollStep = 0;
    enter(POLL);
  }
  setLamp(LAUNCH_LAMP, false);
}

const char *const POLL_LINE[4] = {"RANGE", "WEATHER", "GUIDANCE", "BOOSTER"};

bool anyArmLost() {
  for (int i = 0; i < 3; i++) {
    if (!isOn(ARM_SWITCH[i])) return true;
  }
  return false;
}

void noGo(const char *why) {
  Serial.print("NO-GO: ");
  Serial.println(why);
  for (int i = 0; i < 3; i++) {
    if (!isOn(ARM_SWITCH[i])) {
      confirmed[i] = false;
      setLamp(ARM_LAMP[i], false);
    }
  }
  beep(220, 300);
  enter(CHECKLIST);
}

void runPoll() {
  if (anyArmLost()) {
    noGo("ARMING LOST");
    return;
  }
  if (millis() - phaseStarted < 700) return;
  phaseStarted = millis();
  if (pollStep == 0) {
    showLine(0, "GO/NO-GO POLL");
    showLine(1, "");
    showLine(2, "");
    showLine(3, "");
  }
  if (pollStep < 4) {
    Row line;
    line.s(POLL_LINE[pollStep]);
    while (line.n < 14) line.s(".");
    line.s(" GO");
    const char *row = line.t;
    Serial.println(row);
    // Three stations to a screen, scrolling.
    if (pollStep < 3) {
      showLine(pollStep + 1, row);
    } else {
      showLine(1, lines[2]);
      showLine(2, lines[3]);
      showLine(3, row);
    }
    beep(990, 40);
    pollStep++;
    return;
  }
  showLine(0, "ALL STATIONS GO");
  showLine(1, "GO FOR LAUNCH");
  showLine(2, "");
  showLine(3, "MASTER ARM, LAUNCH");
  Serial.println("GO FOR LAUNCH");
  enter(READY);
}

void runReady() {
  if (anyArmLost()) {
    noGo("ARMING LOST");
    return;
  }
  bool armed = isOn(MASTER_ARM);
  setLamp(LAUNCH_LAMP, armed && blinkOn(600));
  showLine(2, armed ? "MASTER ARM: ARMED" : "MASTER ARM: SAFE");
  showCountdown(10);
  if (pressed(launchButton)) {
    if (!armed) {
      Serial.println("LAUNCH REFUSED: MASTER ARM SAFE");
      showLine(3, "LIFT MASTER ARM");
      beep(220, 250);
    } else if (throttlePercent() > 5) {
      Serial.println("LAUNCH REFUSED: THROTTLE");
      showLine(3, "THROTTLE TO MINIMUM");
      beep(220, 250);
    } else {
      countdown = 10;
      Serial.println("T-10");
      showLine(0, "TERMINAL COUNT");
      showLine(3, "");
      beep(880, 80);
      resetFlight();
      enter(COUNTDOWN);
    }
  }
}

void hold(const char *why) {
  Serial.print("HOLD: ");
  Serial.println(why);
  showLine(0, "HOLD HOLD HOLD");
  showLine(1, why);
  showLine(3, "RECYCLE TO T-10");
  ringOff();
  beep(220, 400);
  setLamp(LAUNCH_LAMP, false);
  enter(READY);
}

void runCountdown() {
  if (!isOn(MASTER_ARM)) {
    hold("MASTER ARM SAFED");
    return;
  }
  if (anyArmLost()) {
    hold("ARMING LOST");
    return;
  }
  setLamp(LAUNCH_LAMP, true);
  showCountdown(countdown);
  showLine(1, Row().s("T-").d(countdown).t);
  if (countdown <= 8) {
    showEngine(1, 0.15 + 0.03 * (8 - countdown));
    showLine(2, "IGNITION SEQUENCE");
  }
  if (countdown <= 3) showLine(3, "THROTTLE TO FULL");
  if (millis() - phaseStarted < 1000) return;
  phaseStarted = millis();
  countdown--;
  if (countdown > 0) {
    Serial.print("T-");
    Serial.println(countdown);
    if (countdown == 8) Serial.println("IGNITION SEQUENCE START");
    beep(880, 80);
  } else {
    showCountdown(0);
    enter(HOLDDOWN);
  }
}

// At T-0 the clamps let go only once the engines make more thrust than the
// rocket weighs: below about 75% throttle it would just sit on the pad.
void runHolddown() {
  int throttle = throttlePercent();
  showEngine(1, 0.3 + 0.7 * throttle / 100.0);
  if (throttle >= 90) {
    showLine(0, "LIFTOFF");
    showLine(1, "");
    showLine(2, "");
    showLine(3, "");
    Serial.println("LIFTOFF");
    beep(1320, 200);
    enter(FLIGHT);
    return;
  }
  showLine(1, "T-0  HOLD-DOWN");
  showLine(2, "THRUST < WEIGHT");
  showLine(3, "THROTTLE TO FULL");
}

// One step of the climb: dt real seconds.
void fly(float dt, int throttle) {
  float t = dt * SPEEDUP;
  flightTime += t;
  float power = throttle / 100.0;
  if (fuel <= 0) power = 0;
  if (phase == STAGING) power = 0;
  if (fault == TANK_FAULT) power *= 0.8;

  // Thrust per kilogram climbs as the propellant burns off.
  float accel = stageNumber == 1 ? 13.5 + 24.5 * (1.0 - fuel) : 9.0 + 21.0 * (1.0 - fuel);
  float burn = stageNumber == 1 ? 1.0 / 150.0 : 1.0 / 360.0;

  // The stick trims the attitude; the autopilot pulls it back slowly unless
  // its computer is the thing that failed.
  float pitchStick = -stick(STICK_Y);
  float yawStick = stick(STICK_X);
  float damping = fault == GUIDANCE_FAULT ? 0.0 : 0.04;
  float wander = fault == GUIDANCE_FAULT ? 1.6 : 0.5;
  attitudeError += (random(-100, 101) / 100.0) * wander + pitchStick * 6.0 * dt - attitudeError * damping * t;
  yawError += (random(-100, 101) / 100.0) * wander * 0.6 + yawStick * 6.0 * dt - yawError * damping * t;
  attitudeError = constrain(attitudeError, -30.0, 30.0);
  yawError = constrain(yawError, -30.0, 30.0);
  steeringSum += fabs(attitudeError) + fabs(yawError);
  steeringTime += 1.0;

  float pitch = constrain(guidancePitch() + attitudeError, 0.0, 90.0);
  float p = pitch * PI / 180.0;
  float lost = cos(yawError * PI / 180.0);

  float r = EARTH_R + altitude;
  float g = MU / (r * r);
  float lift = vAcross * vAcross / r;
  float rho = 1.225 * exp(-altitude / 8500.0);
  float v = speed();
  float q = 0.5 * rho * v * v;
  float drag = 0.5 * rho * v * v * 0.00004;

  float aUp = accel * power * lost * sin(p) - g + lift - (v > 0 ? drag * vUp / v : 0);
  float aAcross = accel * power * lost * cos(p) - (v > 0 ? drag * vAcross / v : 0);
  if (altitude <= 0 && aUp < 0 && flightTime < 10) aUp = 0;
  vUp += aUp * t;
  vAcross += aAcross * t;
  altitude += vUp * t;
  fuel -= burn * power * t / (fault == TANK_FAULT ? 0.8 : 1.0);
  if (fuel < 0) fuel = 0;

  if (q > maxQ) maxQ = q;
  if (!towerCalled && altitude > 150) {
    towerCalled = true;
    callout("TOWER CLEARED");
  }
  if (!maxQCalled && maxQ > 20000 && q < maxQ * 0.97) {
    maxQCalled = true;
    callout(Row().s("MAX Q ").d((long)(maxQ / 1000.0 + 0.5)).s(" kPa").t);
  }
  if (maxQCalled && !throttleUpCalled && q < maxQ * 0.6) {
    throttleUpCalled = true;
    callout("GO AT THROTTLE UP");
  }
  if (!fairingCalled && stageNumber == 2 && altitude > 115000) {
    fairingCalled = true;
    callout("FAIRING SEP");
  }
  if (altitude < 0 && flightTime > 10) {
    altitude = 0;
    abortReason = "LOST ALTITUDE";
    Serial.println("ABORT: LOST ALTITUDE");
    enter(ABORTED);
  }
  if (fault == OVERTEMP_FAULT && throttle >= 70 && millis() - faultStarted > FAULT_LIMIT_MS) {
    // checkFault aborts on the limit; nothing else to do here.
  }
  if (faultsFired < 2 && fault == NO_FAULT && flightTime > faultAt[faultsFired]) {
    raiseFault(faultKind[faultsFired]);
    faultsFired++;
  }
}

void showFlight(int throttle) {
  long alt = (long)(altitude / 100.0);  // tenths of a kilometer
  long vel = (long)(speed() / 10.0);    // hundredths of a km/s
  int pitch = (int)(constrain(guidancePitch() + attitudeError, 0.0, 90.0) + 0.5);
  showLine(0, Row().s("ALT").d(alt / 10, 4).s(".").d(alt % 10).s("km PITCH").d(pitch, 3).t);
  showLine(1, Row().s("VEL").d(vel / 100, 2).s(".").d(vel % 100, 2, false, '0').s(" P").d((int)attitudeError, 3, true).s(" Y").d((int)yawError, 3, true).t);
  showLine(2, Row().s("S").d(stageNumber).s(" FUEL").d((long)(fuel * 100.0 + 0.5), 3).s("% THR").d(throttle, 3).s("%").t);
  if (fault != NO_FAULT) {
    if (!alarmSilenced) {
      showLine(3, Row().s("ALARM ").s(faultName(fault)).t);
    } else {
      showLine(3, faultFix(fault));
    }
  } else if (millis() < messageUntil) {
    showLine(3, message);
  } else if (phase == STAGING) {
    showLine(3, "MECO: PRESS STAGE");
  } else {
    showLine(3, stageNumber == 1 ? "STAGE 1 BURN" : "STAGE 2 BURN");
  }
}

unsigned long lastStep = 0;

void runFlight() {
  unsigned long now = millis();
  float dt = (now - lastStep) / 1000.0;
  if (dt > 0.2) dt = 0.2;
  lastStep = now;
  int throttle = throttlePercent();
  checkFault(throttle);
  if (phase == ABORTED) return;
  fly(dt, throttle);
  if (phase == ABORTED) return;

  showMissionTime(flightTime);
  showFuel(fuel, true);
  showPitch(constrain(guidancePitch() + attitudeError, 0.0, 90.0));
  float power = throttle / 100.0;
  if (fuel <= 0 || phase == STAGING) power = 0;
  if (power > 0) {
    showEngine(stageNumber, 0.25 + 0.75 * power);
  } else {
    ringOff();
  }

  if (phase == FLIGHT && fuel <= 0 && stageNumber == 1) {
    callout("MECO");
    enter(STAGING);
  }
  if (phase == STAGING) {
    setLamp(LAUNCH_LAMP, blinkOn(300));
    if (pressed(launchButton)) {
      Serial.println("STAGE SEP");
      beep(660, 150);
      stageNumber = 2;
      fuel = 1.0;
      setLamp(LAUNCH_LAMP, false);
      callout("SECOND ENGINE START");
      enter(FLIGHT);
    }
  } else {
    setLamp(LAUNCH_LAMP, false);
  }
  workOutOrbit();
  if (stageNumber == 2 && perigee >= TARGET_PERIGEE) {
    ringOff();
    noTone(PIEZO);
    setLamp(ALARM_LAMP, false);
    fault = NO_FAULT;
    Serial.println("SECO");
    Serial.println(Row().s("ORBIT ").d((long)(perigee / 1000.0)).s(" x ").d((long)(apogee / 1000.0)).s(" km").t);
    beep(1320, 120);
    enter(ORBIT);
    return;
  }
  if (stageNumber == 2 && fuel <= 0) {
    abortReason = "SUBORBITAL";
    Serial.println("ABORT: SUBORBITAL");
    enter(ABORTED);
  }
  showFlight(throttle);
}

bool allSafe() {
  for (int i = 0; i < 3; i++) {
    if (isOn(ARM_SWITCH[i])) return false;
  }
  return !isOn(MASTER_ARM);
}

void runEnd() {
  if (phase == ORBIT) {
    showOrbitRing();
    showLine(0, "SECO  NOMINAL ORBIT");
    showLine(1, Row().s("ORBIT ").d((long)(perigee / 1000.0)).s(" x ").d((long)(apogee / 1000.0)).s(" km").t);
    int score = steeringTime > 0 ? 100 - (int)(steeringSum / steeringTime * 4.0) : 100;
    showLine(2, Row().s("STEERING ").d(constrain(score, 0, 100), 3).s("%").t);
    showPitch(0);
  } else {
    ringOff();
    showLine(0, "ABORT");
    showLine(1, abortReason);
    showLine(2, "");
  }
  showFuel(stageNumber == 2 ? fuel : 0, false);
  setLamp(LAUNCH_LAMP, false);
  for (int i = 0; i < 3; i++) setLamp(ARM_LAMP[i], isOn(ARM_SWITCH[i]));
  showLine(3, "SAFE ALL SWITCHES");
  if (allSafe()) {
    Serial.println("VEHICLE SAFED");
    setLamp(ALARM_LAMP, false);
    for (int i = 0; i < 3; i++) confirmed[i] = false;
    showPitch(90);
    clockDisplay.clear();
    showFuel(1.0, false);
    enter(CHECKLIST);
  }
}

void setup() {
  Serial.begin(9600);
  for (int i = 0; i < 3; i++) {
    pinMode(ARM_SWITCH[i], INPUT_PULLUP);
    pinMode(ARM_BUTTON[i], INPUT_PULLUP);
    pinMode(ARM_LAMP[i], OUTPUT);
  }
  pinMode(MASTER_ARM, INPUT_PULLUP);
  pinMode(LAUNCH_BUTTON, INPUT_PULLUP);
  pinMode(ALARM_BUTTON, INPUT_PULLUP);
  pinMode(LAUNCH_LAMP, OUTPUT);
  pinMode(ALARM_LAMP, OUTPUT);
  for (int i = 0; i < 10; i++) pinMode(BAR_PIN[i], OUTPUT);

  lcd.init();
  lcd.backlight();
  for (int r = 0; r < 4; r++) {
    memset(shown[r], ' ', 20);
    shown[r][20] = 0;
    showLine(r, "");
  }
  clockDisplay.setBrightness(5);
  clockDisplay.clear();
  ring.begin();
  ring.show();
  gauge.attach(GAUGE_PIN);
  showPitch(90);
  showFuel(1.0, false);

  randomSeed(analogRead(A15));

  showLine(0, "MOKXI FLIGHT DECK");
  showLine(1, "");
  showLine(2, "");
  showLine(3, "");
  Serial.println("FLIGHT DECK READY");
  for (int r = 0; r < 4; r++) serviceLcd();
  Serial.flush();
  delay(800);
  enter(CHECKLIST);
}

void loop() {
  switch (phase) {
    case CHECKLIST: runChecklist(); break;
    case POLL: runPoll(); break;
    case READY: runReady(); break;
    case COUNTDOWN: runCountdown(); break;
    case HOLDDOWN: runHolddown(); break;
    case FLIGHT:
    case STAGING: runFlight(); break;
    case ORBIT:
    case ABORTED: runEnd(); break;
  }
  if (phase != FLIGHT && phase != STAGING) lastStep = millis();
  serviceLcd();
  // Let the last character of anything printed finish before the chip
  // naps in delay(), so the serial monitor gets every line whole.
  Serial.flush();
  delay(20);
}