control_math.cpp

#include "control_math.h"

#include <math.h>

float clampf(float v, float lo, float hi) {
    if (v < lo) return lo;
    if (v > hi) return hi;
    return v;
}

// fmodf rather than a pair of while loops, and a finiteness guard in front of
// it. Both forms fold the same way over every input the aircraft can produce,
// but "while (error > 180) error -= 360" takes |error|/360 iterations, and never
// terminates at all for an infinity, because inf - 360.0f is still inf. Nothing
// reachable feeds this a non-finite value today - ground_station.cpp rejects
// those on the uplink and getHeading() comes out of atan2 - but this runs at
// 50 Hz on a flight computer with no watchdog, and both a hang and a NaN
// escaping into the motor commands are worse failures than returning zero.
//
// +-180 are left where they are rather than folded onto one side: half a turn is
// the same distance either way, and the caller's sign is as good a choice as any.
float wrapHeadingErrorDeg(float error) {
    if (!isfinite(error)) {
        return 0.0f;
    }

    float folded = fmodf(error, 360.0f);
    if (folded > 180.0f) {
        folded -= 360.0f;
    } else if (folded < -180.0f) {
        folded += 360.0f;
    }
    return folded;
}

void pdInit(PdController *c, float kp, float kd) {
    c->kp = kp;
    c->kd = kd;
    c->prevError = 0.0f;
    c->primed = false;
}

float pdUpdate(PdController *c, float error, float dt) {
    float derivative = 0.0f;

    // On the very first call there is no previous error to difference against,
    // and prevError = 0 would fake a huge step change. Skip the D term instead.
    if (c->primed && dt > 0.0f) {
        derivative = (error - c->prevError) / dt;
    }

    c->prevError = error;
    c->primed = true;

    return c->kp * error + c->kd * derivative;
}