motors.cpp

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

// Teensy 4.1 pins already spoken for: 0/1 Serial1 (GPS), 2 + 9 + 10 LoRa control
// lines, 11-13 the SPI bus the LoRa module rides on, 18/19 I2C for the barometer
// and magnetometer. 7/8 are Serial2 and are kept free for a wired link - nothing
// uses them today, telemetry goes down the LoRa radio. The ESCs take the next
// free block.
static const int MOTOR_PINS[MOTOR_COUNT] = {3, 4, 5, 6};

// Standard hobby ESC pulse range.
static const int ESC_MIN_US = 1000;
static const int ESC_MAX_US = 2000;

static Servo motors[MOTOR_COUNT];

void initMotors() {
    for (int i = 0; i < MOTOR_COUNT; i++) {
        motors[i].attach(MOTOR_PINS[i]);
        motors[i].writeMicroseconds(ESC_MIN_US);   // sit at idle until commanded
    }
}

void setMotorPower(int motorID, float power) {
    if (motorID < 0 || motorID >= MOTOR_COUNT) {
        return;
    }

    // A NaN is false against both bounds below, and (int)NaN on the way to
    // writeMicroseconds() is undefined behaviour. Nothing upstream can produce
    // one today - ground_station.cpp refuses a non-finite ALT or HDG - but this
    // is the last layer before the wire, so the guarantee is made here instead
    // of trusted two modules away. Idle is the only reading of a throttle
    // command that is not a number. Note the boundary is a cliff, not a ramp:
    // the largest finite float still clamps to full throttle and the next
    // representable value up commands idle. telemetry.cpp's fixedPoint() falls
    // the same way, so the downlink at least agrees with the ESC.
    if (!isfinite(power) || power < 0.0f) {
        power = 0.0f;
    } else if (power > 1.0f) {
        power = 1.0f;
    }

    // Rounded rather than truncated, matching setServoAngle(). power is
    // non-negative by here, and the endpoints still land on exactly 1000/2000.
    motors[motorID].writeMicroseconds(ESC_MIN_US + (int)(power * (ESC_MAX_US - ESC_MIN_US) + 0.5f));
}

void stopAllMotors() {
    for (int i = 0; i < MOTOR_COUNT; i++) {
        motors[i].writeMicroseconds(ESC_MIN_US);
    }
}