flight_control_vertical.cpp

#include <math.h>

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

// Altitude hold. None of these gains have been tuned against the real airframe.
// HOVER_THROTTLE is the throttle that holds the aircraft's own weight and has to
// be measured on a thrust stand before any of this leaves the ground.
#define KP_ALTITUDE      1.2f
#define KD_ALTITUDE      0.5f
#define MAX_CLIMB_RATE   2.0f    // m/s, how fast the setpoint is allowed to walk
#define HOVER_THROTTLE   0.5f

static float desiredAltitude = 0.0f;
static float holdAltitude = 0.0f;
static float lastThrottle = 0.0f;

// Starts the loop holding wherever the aircraft currently is, so arming never
// commands a climb or a drop on its own.
void initVerticalController() {
    desiredAltitude = getAltitude();
    holdAltitude = desiredAltitude;
}

void updateVerticalControl(float dt) {
    // Walk the working setpoint towards the commanded one at MAX_CLIMB_RATE. A
    // "climb to 100 m" command otherwise arrives as a 100 m step, and the loop
    // just pins the throttle at full until it gets there.
    float step = MAX_CLIMB_RATE * dt;
    holdAltitude += clampf(desiredAltitude - holdAltitude, -step, step);

    // Metres, both of them. The barometer's hPa is converted exactly once, in
    // sensors.cpp, and nothing in this file ever touches the driver directly.
    float error = holdAltitude - getAltitude();

    // PD, with the damping taken from the measured climb rate rather than from
    // differencing the error. getVerticalSpeed() is already low-passed, whereas
    // the difference of a barometer at 50 Hz is mostly noise, and a D gain on
    // that chatters the throttle between zero and full every cycle.
    float throttle = HOVER_THROTTLE + KP_ALTITUDE * error - KD_ALTITUDE * getVerticalSpeed();
    throttle = clampf(throttle, 0.0f, 1.0f);

    // All four rotors run together. Trimming them against each other for pitch
    // and roll needs an IMU, and this airframe does not carry one.
    for (int motor = 0; motor < MOTOR_COUNT; motor++) {
        setMotorPower(motor, throttle);
    }
    lastThrottle = throttle;
}

void setDesiredAltitude(float altitude_m) {
    // A NaN here rides holdAltitude into the throttle, and clampf(NaN, 0, 1)
    // hands it back unchanged - both of its comparisons are false - so what
    // reaches setMotorPower() is not a throttle at all. The uplink parser
    // refuses non-finite values already, but that is a module away.
    if (!isfinite(altitude_m)) {
        return;
    }
    desiredAltitude = altitude_m;
}

float verticalThrottle() {
    return lastThrottle;
}