ESP32 Quadcopter Flight Controller

#include <Arduino.h>
#include <Wire.h>
#include <MPU6050.h>

// X-quad viewed from above with nose forward. MPU6050 +X faces forward, +Y right.

// Forward declarations
float clampFloat(float v, float low, float high);
float mapChannel(uint16_t v, float low, float high);
void writeEsc(uint8_t channel, uint16_t us);
void stopMotors();
void readSbus();
void calibrateGyro();

constexpr int IMU_SDA = 21;
constexpr int IMU_SCL = 22;
constexpr int SBUS_RX = 16;
constexpr int ESC_FL = 25;
constexpr int ESC_FR = 26;
constexpr int ESC_RL = 27;
constexpr int ESC_RR = 33;
constexpr uint16_t ESC_MIN_US = 1000;
constexpr uint16_t ESC_MAX_US = 2000;
constexpr uint16_t ESC_IDLE_US = 1070;
constexpr uint32_t ESC_PWM_HZ = 400;
constexpr uint32_t CONTROL_PERIOD_US = 4000;
constexpr uint32_t RX_TIMEOUT_MS = 120;
constexpr int SBUS_CENTER = 992;

MPU6050 imu;
HardwareSerial SbusSerial(2);
uint16_t rc[16] = {SBUS_CENTER};
bool sbusFailsafe = true;
bool sbusFrameLost = true;
uint32_t lastRxMs = 0;
float gyroXBias = 0.0f, gyroYBias = 0.0f;
float rollDeg = 0.0f, pitchDeg = 0.0f, rollI = 0.0f, pitchI = 0.0f;
float prevRollError = 0.0f, prevPitchError = 0.0f;
uint32_t lastControlUs = 0;
bool armed = false;

float clampFloat(float v, float low, float high) { return v < low ? low : (v > high ? high : v); }
float mapChannel(uint16_t v, float low, float high) {
  float n = clampFloat((static_cast<float>(v) - 172.0f) / 1639.0f, 0.0f, 1.0f);
  return low + n * (high - low);
}
void writeEsc(uint8_t channel, uint16_t us) {
  us = constrain(us, ESC_MIN_US, ESC_MAX_US);
  uint32_t duty = (static_cast<uint32_t>(us) * ESC_PWM_HZ * 65535UL) / 1000000UL;
  ledcWrite(channel, duty);
}
void stopMotors() { for (uint8_t i = 0; i < 4; ++i) writeEsc(i, ESC_MIN_US); }

void readSbus() {
  static uint8_t frame[25];
  static uint8_t index = 0;
  while (SbusSerial.available()) {
    uint8_t b = static_cast<uint8_t>(SbusSerial.read());
    if (index == 0 && b != 0x0F) continue;
    frame[index++] = b;
    if (index != 25) continue;
    index = 0;
    rc[0] = (frame[1] | frame[2] << 8) & 0x07FF;
    rc[1] = (frame[2] >> 3 | frame[3] << 5) & 0x07FF;
    rc[2] = (frame[3] >> 6 | frame[4] << 2 | frame[5] << 10) & 0x07FF;
    rc[3] = (frame[5] >> 1 | frame[6] << 7) & 0x07FF;
    rc[4] = (frame[6] >> 4 | frame[7] << 4) & 0x07FF;
    sbusFrameLost = frame[23] & 0x04;
    sbusFailsafe = frame[23] & 0x08;
    lastRxMs = millis();
  }
}

void calibrateGyro() {
  int32_t sx = 0, sy = 0;
  for (int i = 0; i < 600; ++i) {
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sx += gx; sy += gy;
    delay(5);
  }
  gyroXBias = sx / 600.0f; gyroYBias = sy / 600.0f;
}

void setup() {
  Serial.begin(115200);
  Wire.begin(IMU_SDA, IMU_SCL);
  Wire.setClock(400000);
  imu.initialize();
  ledcSetup(0, ESC_PWM_HZ, 16); ledcAttachPin(ESC_FL, 0);
  ledcSetup(1, ESC_PWM_HZ, 16); ledcAttachPin(ESC_FR, 1);
  ledcSetup(2, ESC_PWM_HZ, 16); ledcAttachPin(ESC_RL, 2);
  ledcSetup(3, ESC_PWM_HZ, 16); ledcAttachPin(ESC_RR, 3);
  stopMotors();
  // SBUS: 100000 baud, 8E2, inverted. Input must be no more than 3.3 V.
  SbusSerial.begin(100000, SERIAL_8E2, SBUS_RX, -1, true);
  if (!imu.testConnection()) {
    Serial.println("MPU6050 missing: motors locked off.");
    while (true) { stopMotors(); delay(100); }
  }
  Serial.println("Hold aircraft motionless for gyro calibration.");
  calibrateGyro();
  lastControlUs = micros();
}

void loop() {
  readSbus();
  uint32_t now = micros();
  if (now - lastControlUs < CONTROL_PERIOD_US) return;
  float dt = (now - lastControlUs) / 1000000.0f;
  lastControlUs = now;
  bool receiverHealthy = (millis() - lastRxMs < RX_TIMEOUT_MS) && !sbusFailsafe && !sbusFrameLost;
  float throttle = mapChannel(rc[2], 0.0f, 1.0f);
  bool armSwitch = rc[4] > 1200;
  if (!receiverHealthy || !armSwitch) armed = false;
  else if (!armed && throttle < 0.05f) { armed = true; rollI = 0; pitchI = 0; }
  if (!armed) { stopMotors(); return; }

  int16_t ax, ay, az, gxRaw, gyRaw, gzRaw;
  imu.getMotion6(&ax, &ay, &az, &gxRaw, &gyRaw, &gzRaw);
  float gx = (gxRaw - gyroXBias) / 131.0f;
  float gy = (gyRaw - gyroYBias) / 131.0f;
  float accelRoll = atan2f(static_cast<float>(ay), static_cast<float>(az)) * 180.0f / PI;
  float accelPitch = atan2f(-static_cast<float>(ax), sqrtf(static_cast<float>(ay) * ay + static_cast<float>(az) * az)) * 180.0f / PI;
  rollDeg = 0.98f * (rollDeg + gx * dt) + 0.02f * accelRoll;
  pitchDeg = 0.98f * (pitchDeg + gy * dt) + 0.02f * accelPitch;

  float rollError = mapChannel(rc[0], -20.0f, 20.0f) - rollDeg;
  float pitchError = mapChannel(rc[1], -20.0f, 20.0f) - pitchDeg;
  rollI = clampFloat(rollI + rollError * dt, -25, 25);
  pitchI = clampFloat(pitchI + pitchError * dt, -25, 25);
  float rollOut = 5.0f * rollError + 1.2f * rollI + 0.08f * (rollError - prevRollError) / dt;
  float pitchOut = 5.0f * pitchError + 1.2f * pitchI + 0.08f * (pitchError - prevPitchError) / dt;
  prevRollError = rollError; prevPitchError = pitchError;
  float yawOut = mapChannel(rc[3], -80.0f, 80.0f);
  float base = ESC_IDLE_US + throttle * (ESC_MAX_US - ESC_IDLE_US);
  writeEsc(0, static_cast<uint16_t>(clampFloat(base + pitchOut - rollOut + yawOut, ESC_IDLE_US, ESC_MAX_US)));
  writeEsc(1, static_cast<uint16_t>(clampFloat(base + pitchOut + rollOut - yawOut, ESC_IDLE_US, ESC_MAX_US)));
  writeEsc(2, static_cast<uint16_t>(clampFloat(base - pitchOut - rollOut - yawOut, ESC_IDLE_US, ESC_MAX_US)));
  writeEsc(3, static_cast<uint16_t>(clampFloat(base - pitchOut + rollOut + yawOut, ESC_IDLE_US, ESC_MAX_US)));
}