flight_control_horizontal.cpp

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

#include "flight_control_horizontal.h"
#include "control_math.h"
#include "motors.h"
#include "sensors.h"

// Heading comes from the magnetometer and speed from the GPS. There is no
// position hold: flying to a latitude/longitude needs rate terms the GPS driver
// does not expose, and a made-up rate term is worse than no position loop.
//
// Yaw authority in cruise is differential thrust between the left and right
// rotor pairs, which by this point are tilted forward. Gains are untuned.
#define KP_HEADING       0.010f   // degrees of error -> throttle differential
#define KD_HEADING       0.002f
#define KP_SPEED         0.05f    // m/s of error -> throttle
#define MAX_YAW_DIFFERENTIAL 0.15f
#define CRUISE_THROTTLE  0.45f

// The airframe's motor wiring order is written down nowhere in this repo. This
// is the convention the rest of the firmware assumes; check it against the ESC
// harness before powering anything up.
#define MOTOR_FRONT_LEFT   0
#define MOTOR_FRONT_RIGHT  1
#define MOTOR_REAR_LEFT    2
#define MOTOR_REAR_RIGHT   3

static PdController headingPd;

static float desiredHeading = 0.0f;
static float desiredSpeed = 0.0f;
static float lastThrottle[MOTOR_COUNT] = {0.0f, 0.0f, 0.0f, 0.0f};

// Cleared at every hand-off, so a cruise leg begun with no heading source still
// says so once rather than inheriting a previous leg's silence.
static bool headingLossAnnounced = false;

void initHorizontalController() {
    pdInit(&headingPd, KP_HEADING, KD_HEADING);
    desiredHeading = getHeading();
    desiredSpeed = getGroundSpeed();
    headingLossAnnounced = false;
}

void updateHorizontalControl(float dt) {
    // Once the magnetometer stops answering, getHeading() repeats its last
    // reading for the rest of the flight, so desiredHeading - getHeading() is a
    // constant and the PD holds a fixed differential into a turn that can never
    // converge - a circle flown until the battery gives out. Stop steering
    // instead: equal thrust on both pairs holds whatever heading it had, which
    // is the only thing about the heading that is still true. gpsCourse() is not
    // a stand-in for the lost reading and gps.h says why.
    float yaw = 0.0f;
    if (headingValid()) {
        // Both headings are 0..360, so the raw difference can be 350 when the
        // real turn is 10 the other way.
        float headingError = wrapHeadingErrorDeg(desiredHeading - getHeading());
        yaw = pdUpdate(&headingPd, headingError, dt);
        yaw = clampf(yaw, -MAX_YAW_DIFFERENTIAL, MAX_YAW_DIFFERENTIAL);
    } else if (!headingLossAnnounced) {
        headingLossAnnounced = true;
        Serial.println("Heading source down: holding the last heading, no yaw command");
    }

    // Speed is proportional only. The GPS reports at a few Hz and holds its last
    // value in between, so differencing it at 50 Hz sees a step every time a fix
    // lands and throws a one-cycle throttle spike for each one. The cruise
    // feedforward carries the steady state instead.
    //
    // With no fix there is nothing to close the loop on. getGroundSpeed() reports
    // a hard 0 m/s rather than a stale rate, so that the transition gate fails
    // closed - but putting that through the proportional term reads a sentinel as
    // a measurement, and at a 15 m/s setpoint it commands full throttle one cycle
    // after the fix drops. Coast on the feedforward until the GPS comes back.
    float thrust = CRUISE_THROTTLE;
    if (hasGpsFix()) {
        thrust += KP_SPEED * (desiredSpeed - getGroundSpeed());
    }
    thrust = clampf(thrust, 0.0f, 1.0f);

    // Positive heading error means turn right, so the left pair works harder.
    float left = clampf(thrust + yaw, 0.0f, 1.0f);
    float right = clampf(thrust - yaw, 0.0f, 1.0f);

    setMotorPower(MOTOR_FRONT_LEFT, left);
    setMotorPower(MOTOR_REAR_LEFT, left);
    setMotorPower(MOTOR_FRONT_RIGHT, right);
    setMotorPower(MOTOR_REAR_RIGHT, right);

    lastThrottle[MOTOR_FRONT_LEFT] = left;
    lastThrottle[MOTOR_REAR_LEFT] = left;
    lastThrottle[MOTOR_FRONT_RIGHT] = right;
    lastThrottle[MOTOR_REAR_RIGHT] = right;
}

void setDesiredHeading(float heading_deg) {
    // Same guard, for a quieter failure. wrapHeadingErrorDeg() folds a
    // non-finite error to zero, so the ESCs are safe either way - but the loop
    // is then left reporting no heading error for the rest of the flight,
    // holding nothing at all.
    if (!isfinite(heading_deg)) {
        return;
    }
    desiredHeading = heading_deg;
}

float horizontalThrottle(int motorID) {
    if (motorID < 0 || motorID >= MOTOR_COUNT) {
        return 0.0f;
    }
    return lastThrottle[motorID];
}