control_math.h

#ifndef CONTROL_MATH_H
#define CONTROL_MATH_H

// The maths behind both control loops. Deliberately free of Arduino headers so
// it can be built and run on a desktop - see test/test_control_math.cpp.

// clampf(NaN, lo, hi) returns NaN, because both comparisons against a NaN are
// false. That is on purpose and pinned by a test. Substituting a bound would
// turn clampf(yaw, -MAX_YAW_DIFFERENTIAL, +MAX_YAW_DIFFERENTIAL) into a command
// for a hard turn - a plausible-looking number is worse here than an obviously
// broken one. The guard that has to hold belongs at the actuator boundary, and
// lives in setMotorPower() and setServoAngle().
float clampf(float v, float lo, float hi);

// Folds a heading error into -180..180. Without this a 350 deg error commands a
// hard turn the long way round instead of a 10 deg nudge the other way.
float wrapHeadingErrorDeg(float error);

struct PdController {
    float kp;
    float kd;
    float prevError;
    bool  primed;      // false until the first update, so the first call has no derivative
};

void  pdInit(PdController *c, float kp, float kd);

// dt is in seconds and comes from the flight computer's cycle timer.
float pdUpdate(PdController *c, float error, float dt);

#endif // CONTROL_MATH_H