servos.cpp

#include "servos.h"
#include <Servo.h>
#include <Arduino.h>
#include <math.h>

// The old single servo sat on pin 11, which on a Teensy 4.1 is SPI MOSI, so it
// was fighting the LoRa module rather than the motors. With the ESCs now on 3-6,
// the tilt servos go to 20-23, keeping A0-A3 (14-17) free for analog sensing later.
static const int SERVO_PINS[SERVO_COUNT] = {20, 21, 22, 23};

// Servo::attach() sets the channel to DEFAULT_PULSE_WIDTH (1500 us, i.e. 90 deg)
// and starts the timer immediately, so there is no such thing as an attached
// channel that is not being driven. The only choice is whose default it is, and
// an explicit one in this file beats an implicit one in the library. Centre is
// the least committal place to leave a channel nothing has claimed yet - the
// flight computer drives 0 and 1 to the hover tilt right after this returns, and
// 2 and 3 are spare.
static const float SERVO_INIT_DEG = 90.0f;

static Servo servos[SERVO_COUNT];

void initServos() {
    for (int i = 0; i < SERVO_COUNT; i++) {
        servos[i].attach(SERVO_PINS[i]);
        setServoAngle(i, SERVO_INIT_DEG);
    }
}

void setServoAngle(int servoID, float angle_deg) {
    if (servoID < 0 || servoID >= SERVO_COUNT) {
        return;
    }

    // Same hole as setMotorPower(), opposite answer. A motor's safe state is
    // off; a tilt channel's is wherever it already is - falling back to 0 deg
    // would slam the tilt pair onto the hover stop at full authority, and
    // mid-cruise that is the bigger event. So the command is dropped and the
    // channel keeps pulsing where it was until something sends a finite angle.
    // That is not the same as "until the next cycle": setTilt() in
    // flight_computer.cpp stores the angle before calling here and then ramps
    // from its own copy, so one non-finite value there would freeze the pair
    // for the rest of the flight, with the downlink still reporting 0.0 deg.
    // Nothing upstream can produce one today; if anything ever does, that latch
    // is the thing to fix rather than this guard.
    if (!isfinite(angle_deg)) {
        return;
    }

    if (angle_deg < 0.0f) {
        angle_deg = 0.0f;
    } else if (angle_deg > 180.0f) {
        angle_deg = 180.0f;
    }

    servos[servoID].write((int)(angle_deg + 0.5f));
}