flight_computer.cpp

// The mode machine, and the cycle everything else hangs off. This module owns
// arming, the mode transitions, the servos and the only call to
// updateSensors() - the control loops take a dt and read the sensor facade.
//
// updateFlightComputer(), in order, at the 50 Hz main.cpp holds it to:
//
//   dt         measured, or the nominal period if the clock stalled or jumped
//   sensors    one poll, refreshing every value the loops are about to read
//   health     one line, once, if the sensors have gone bad since arming
//   uplink     the ground station command, if a packet arrived this cycle
//   failsafe   disarm if the link has gone quiet
//   fly        the control loop belonging to the current mode
//   telemetry  one frame, rate limited inside sendTelemetry()
//
// TRANSITION is the only mode running two of those at once: the tilt ramp and
// updateVerticalControl(), since the lift rotors carry the aircraft the whole
// way through it.
//
// What the aircraft decided goes out on the debug serial port and nowhere else.
// The four mode-entry lines share one shape, "<MODE>: <why>", and the refusals
// share another, "<THING> refused: <why>". Disarming is the exception: it
// prints "Disarmed: <why>" rather than "IDLE: <why>", because that is the word
// an operator reading the log is looking for. The health line follows the
// mode-entry shape without being one - "Sensors unhealthy: <what stopped>".

#include <Arduino.h>
#include <math.h>

#include "flight_computer.h"
#include "flight_control_horizontal.h"
#include "flight_control_vertical.h"
#include "ground_station.h"
#include "motors.h"
#include "sensors.h"
#include "servos.h"
#include "telemetry.h"

// Tilt servo IDs. Same caveat as the motor IDs in flight_control_horizontal.cpp:
// the harness order is not documented, so check it before powering up.
#define TILT_SERVO_LEFT   0
#define TILT_SERVO_RIGHT  1

#define TILT_HOVER_DEG    0.0f     // rotors vertical, they carry the whole aircraft
#define TILT_CRUISE_DEG   90.0f    // rotors forward, the wing carries it

// The servos walk at a fixed angular rate rather than over a fixed duration, so
// that turning a ramp round costs the travel it actually has to undo. Timing the
// ramp end to end instead restarts the clock without shortening the distance: an
// abort one second out would still take a full six seconds to put fifteen
// degrees of tilt back.
#define TILT_RATE_DEG_PER_S  15.0f   // 90 deg of hover-to-cruise travel in 6 s

// The two servo channels the airframe does not use. servos.cpp parks every
// channel on attach; commanding them from here as well means the downlink
// reports an angle this file sent rather than one it assumed.
#define SPARE_SERVO_DEG   90.0f

// The wing has to be flying before the lift loop is allowed to stand down. This
// is a guess at the airframe's minimum flying speed and has never been checked.
#define TRANSITION_MIN_GROUND_SPEED  12.0f   // m/s

// A cycle far away from 50 Hz means something blocked. Hand the PD loops the
// nominal period rather than a dt that makes their derivative meaningless.
#define NOMINAL_CYCLE_DT  0.02f
#define MAX_CYCLE_DT      0.2f

static FlightMode currentMode = IDLE;
static bool armed = false;
static bool radioUp = false;

static float tiltAngle = TILT_HOVER_DEG;

// 32 bits to match main.cpp's rollover reasoning: the subtraction below is what
// has to stay correct across the millis() wrap, not the absolute value.
static uint32_t lastCycleMs = 0;

static bool transitionToCruise = false;

// One line per arming, not one per cycle. Cleared in arm() so a second sortie
// after the part recovers at boot still gets told.
static bool unhealthyAnnounced = false;

// Telemetry wants the per-motor commands, and motors.* keeps no copy of what it
// was last told, so each loop reports its own output and the flight computer
// asks whichever one is currently flying the aircraft.
static float commandedMotorPower(int motorID) {
    switch (currentMode) {
        case VERTICAL:
        case TRANSITION:
            return verticalThrottle();
        case HORIZONTAL:
            return horizontalThrottle(motorID);
        case IDLE:
            break;
    }
    return 0.0f;
}

// The tilt pair is the only servo the firmware flies. The spare pair is set once
// in initFlightComputer() and never moved, so it reports the angle it is holding
// - a channel parked at centre showing up as 0 on the downlink would be a lie
// about where the horn actually is.
static float commandedServoAngle(int servoID) {
    if (servoID == TILT_SERVO_LEFT || servoID == TILT_SERVO_RIGHT) {
        return tiltAngle;
    }
    return SPARE_SERVO_DEG;
}

static void setTilt(float angle_deg) {
    tiltAngle = angle_deg;
    setServoAngle(TILT_SERVO_LEFT, angle_deg);
    setServoAngle(TILT_SERVO_RIGHT, angle_deg);
}

// The tilt goes back to hover in one cycle rather than on a ramp. Deliberate on
// the bench, where a defined resting position beats leaving the rotors wherever
// the failure caught them; in cruise it is a full-authority servo step at the
// same instant thrust goes to zero, which is one more reason the failsafe below
// wants replacing with a controlled descent before this ever flies.
static void disarm(const char *reason) {
    stopAllMotors();
    setTilt(TILT_HOVER_DEG);
    armed = false;
    currentMode = IDLE;

    Serial.print("Disarmed: ");
    Serial.println(reason);
}

static void arm() {
    if (armed) {
        return;
    }
    if (!radioUp) {
        Serial.println("Arm refused: radio never came up");
        return;
    }
    if (!sensorsHealthy()) {
        Serial.println("Arm refused: sensors unhealthy");
        return;
    }

    // Resets the loop to hold whatever altitude it is sitting at. Arming must not
    // command a climb by itself - the ground station asks for that with SET_ALTITUDE.
    initVerticalController();
    armed = true;
    currentMode = VERTICAL;
    unhealthyAnnounced = false;

    Serial.println("VERTICAL: armed");
}

static void beginTransition(bool toCruise) {
    transitionToCruise = toCruise;
    currentMode = TRANSITION;

    Serial.println(toCruise ? "TRANSITION: ramping to cruise"
                            : "TRANSITION: ramping back to hover");
}

// The transition is open loop. Nothing measures where the rotors actually got
// to - the servos are walked between the two angles at a fixed rate and trusted
// to have kept up. It has not been bench-tested and it has not been flown.
static void updateTransition(float dt) {
    float target = transitionToCruise ? TILT_CRUISE_DEG : TILT_HOVER_DEG;
    float remaining = target - tiltAngle;
    float step = TILT_RATE_DEG_PER_S * dt;
    bool rampDone = fabsf(remaining) <= step;

    // The last step lands exactly on the target rather than a float's breadth
    // short of it, so finishing the ramp never comes down to an epsilon.
    setTilt(rampDone ? target : tiltAngle + (remaining > 0.0f ? step : -step));

    // The lift rotors keep holding altitude through the whole ramp. Until the
    // wing is flying they are the only thing holding the aircraft up.
    updateVerticalControl(dt);

    if (!rampDone) {
        return;
    }

    if (!transitionToCruise) {
        currentMode = VERTICAL;
        Serial.println("VERTICAL: ramp finished");
        return;
    }

    // hasGpsFix() is checked as well as the speed because this is the only
    // measurement standing between the lift rotors and being switched off.
    if (hasGpsFix() && getGroundSpeed() >= TRANSITION_MIN_GROUND_SPEED) {
        // Resetting the loop picks up the current heading and speed, so cruise
        // starts out holding whatever the aircraft is already doing.
        initHorizontalController();
        currentMode = HORIZONTAL;

        // Fly the first cruise cycle now rather than next time round.
        // updateVerticalControl() already wrote hover throttle to the ESCs this
        // cycle, so leaving it means they hold a hover command for one cycle
        // past the hand-off, and this cycle's downlink frame reports a cruise
        // throttle nothing ever commanded.
        updateHorizontalControl(dt);
        Serial.println("HORIZONTAL: flying speed confirmed");
    } else {
        // Either the aircraft never built up enough speed for the wing to take
        // over, or the GPS cannot say whether it did. Walk the rotors back up
        // rather than hand over to a loop that may not be able to hold it.
        Serial.println("Transition aborted: no flying speed confirmed");
        beginTransition(false);
    }
}

static void handleCommand(const GroundStationCommand &cmd) {
    switch (cmd.type) {
        case CMD_ARM:
            arm();
            break;

        case CMD_DISARM:
            disarm("ground station command");
            break;

        // Cruise has no altitude authority - no vertical loop running, no pitch
        // surface - and the setpoint would not survive the trip back either,
        // since coming off the wing re-seats the loop. Refuse it out loud rather
        // than bank a number nothing will ever act on.
        case CMD_SET_ALTITUDE:
            if (currentMode == HORIZONTAL) {
                Serial.println("ALT refused: cruise does not hold altitude, ask again from hover");
            } else {
                setDesiredAltitude(cmd.value);
            }
            break;

        // The mirror of the above. Outside cruise there is no heading loop
        // running at all - updateVerticalControl() commands all four lift
        // rotors identically, so hover has no yaw authority for a setpoint to
        // act on - and initHorizontalController() captures the current heading
        // at the hand-off, so anything banked earlier is overwritten there.
        //
        // The second refusal is the magnetometer's. With no heading source the
        // cruise loop has already stood its yaw term down, so a setpoint here
        // would be stored and never flown. Two messages rather than one,
        // because "wrong mode" and "no compass" are different problems and only
        // one of them is worth waiting out.
        case CMD_SET_HEADING:
            if (currentMode != HORIZONTAL) {
                Serial.println("HDG refused: only cruise holds a heading, ask again after the transition");
            } else if (!headingValid()) {
                Serial.println("HDG refused: the magnetometer stopped answering, cruise is not steering");
            } else {
                setDesiredHeading(cmd.value);
            }
            break;

        // Both mode commands are accepted mid-ramp as well as from the settled
        // mode at either end: the ramp walks from wherever the servos have got
        // to, so reversing one is just another ramp. Without that the only way
        // out of a transition is DISARM, which cuts all four motors, and the
        // speed gate does not get a vote until the ramp has finished.
        //
        // A refusal is announced. A mode command that does nothing and says
        // nothing leaves the operator watching an aircraft that has ignored
        // them, and the debug serial line is the only place they could find out.
        //
        // The health gate is here and nowhere else. Cruise holds no altitude at
        // all, and the speed-gate abort at the end of updateTransition() hands
        // the aircraft back to a vertical loop closing on a number nothing is
        // measuring any more. Coming back the other way is never refused: hover
        // on a frozen altitude is bad, and it is still better than the wing. And
        // it gates the command, not the hand-off - a barometer that quits with
        // the ramp already running still ends up in cruise, because turning a
        // running ramp round is a change to what an aircraft in the air is doing.
        case CMD_MODE_HORIZONTAL:
            if (!sensorsHealthy()) {
                Serial.println("MODE HORIZONTAL refused: sensors unhealthy, and cruise holds no altitude");
            } else if (armed && (currentMode == VERTICAL ||
                                 (currentMode == TRANSITION && !transitionToCruise))) {
                beginTransition(true);
            } else {
                Serial.println("MODE HORIZONTAL refused: not armed, or already heading for cruise");
            }
            break;

        case CMD_MODE_VERTICAL:
            if (armed && (currentMode == HORIZONTAL ||
                          (currentMode == TRANSITION && transitionToCruise))) {
                // Re-seat the loop on where the aircraft actually is. Nothing
                // has held altitude since it went onto the wing, so its working
                // setpoint is the one it froze at before the cruise leg, and the
                // aircraft has drifted since. Against KP_ALTITUDE that gap is
                // full throttle on all four rotors while they still point
                // forward. Resuming the old commanded altitude has the same
                // problem more slowly, so the climb back is the operator's to
                // ask for once the rotors are upright.
                //
                // Only from HORIZONTAL - the speed-gate abort at the end of
                // updateTransition() lands on beginTransition(false) too, and
                // there the vertical loop has been running all along.
                if (currentMode == HORIZONTAL) {
                    initVerticalController();
                }
                beginTransition(false);
            } else {
                Serial.println("MODE VERTICAL refused: not armed, or already heading for hover");
            }
            break;

        case CMD_NONE:
            break;
    }
}

void initFlightComputer() {
    // Stated rather than assumed. These are file statics, so at power-up they
    // are already zero - but that only lands on IDLE because IDLE happens to be
    // the first enumerator, and someone reordering FlightMode would have the
    // aircraft boot armed and flying a mode nobody selected.
    currentMode = IDLE;
    armed = false;
    transitionToCruise = false;
    unhealthyAnnounced = false;

    initSensors();
    initMotors();
    initServos();
    initTelemetry();

    stopAllMotors();
    for (int servo = TILT_SERVO_RIGHT + 1; servo < SERVO_COUNT; servo++) {
        setServoAngle(servo, SPARE_SERVO_DEG);
    }
    setTilt(TILT_HOVER_DEG);

    initVerticalController();
    initHorizontalController();

    radioUp = initGroundStation();
    if (!radioUp) {
        Serial.println("Ground station radio did not come up - the aircraft will refuse to arm");
    }

    lastCycleMs = (uint32_t)millis();
}

void updateFlightComputer() {
    uint32_t now = (uint32_t)millis();
    float dt = (float)(now - lastCycleMs) / 1000.0f;
    lastCycleMs = now;
    if (dt <= 0.0f || dt > MAX_CYCLE_DT) {
        dt = NOMINAL_CYCLE_DT;
    }

    updateSensors();

    // Until this, arm() was the only reader of sensorsHealthy(), so a barometer
    // that quit at 60 m cost the aircraft nothing but one line from sensors.cpp
    // about the part. getAltitude() repeats its last reading from here on, which
    // is why the message names the reading rather than what any one mode does
    // with it - the lift loop is closing on it in hover and on the ramp, and in
    // cruise it is what the way home hands the aircraft back to. Nothing disarms,
    // cuts the motors or starts a descent on an aircraft that is already up:
    // which of those is survivable depends on the airframe, and on how much of
    // the flight is left, and this has never been on a bench.
    if (armed && !sensorsHealthy() && !unhealthyAnnounced) {
        unhealthyAnnounced = true;
        Serial.println("Sensors unhealthy: altitude has stopped updating");
    }

    if (groundStationCommandReceived()) {
        handleCommand(getGroundStationCommand());
    }

    // Link-loss failsafe. Cutting the motors is right on the bench and wrong in
    // the air, where a real failsafe would fly a controlled descent to a landing.
    // Until this firmware has actually flown, stopping is the safer default.
    if (armed && !groundStationLinkAlive()) {
        disarm("ground station link lost");
    }

    switch (currentMode) {
        case IDLE:
            break;
        case VERTICAL:
            updateVerticalControl(dt);
            break;
        case TRANSITION:
            updateTransition(dt);
            break;
        case HORIZONTAL:
            updateHorizontalControl(dt);
            break;
    }

    // sendTelemetry rate limits itself - the radio cannot carry a frame per cycle.
    sendTelemetry(getFlightData());
}

FlightData getFlightData() {
    FlightData data;
    data.altitude = getAltitude();
    data.verticalSpeed = getVerticalSpeed();
    data.heading = getHeading();
    data.speed = getGroundSpeed();
    data.latitude = getLatitude();
    data.longitude = getLongitude();

    for (int motor = 0; motor < MOTOR_COUNT; motor++) {
        data.motorPower[motor] = commandedMotorPower(motor);
    }
    for (int servo = 0; servo < SERVO_COUNT; servo++) {
        data.servoAngles[servo] = commandedServoAngle(servo);
    }

    data.mode = currentMode;
    data.armed = armed;
    return data;
}