ESP32 Quadcopter Controller

#include <Arduino.h>
#include <WiFi.h>
#include <WebServer.h>
#include <Wire.h>
#include <ESP32Servo.h>
#include <math.h>

#define ESC_FL_PIN 25
#define ESC_FR_PIN 26
#define ESC_RR_PIN 27
#define ESC_RL_PIN 33
#define MPU_SDA_PIN 21
#define MPU_SCL_PIN 22

const char *AP_NAME = "Quad-Setup";
const char *AP_PASSWORD = "drone-setup";
const uint8_t MPU_ADDR = 0x68;
const int ESC_STOP_US = 1000;
const int ESC_MAX_US = 1500;       // Deliberately limited for initial tethered tests.
const uint32_t COMMAND_TIMEOUT_MS = 400;
const float MAX_TILT_DEG = 35.0f;
const float P_GAIN = 3.5f;

Servo escFL, escFR, escRR, escRL;
WebServer server(80);

bool armed = false;
float throttleCommand = 0.0f;
float rollCommand = 0.0f;
float pitchCommand = 0.0f;
float yawCommand = 0.0f;
float rollDeg = 0.0f;
float pitchDeg = 0.0f;
uint32_t lastCommandMs = 0;
uint32_t lastImuUs = 0;

const char INDEX_HTML[] PROGMEM = R"HTML(
<!doctype html><html><head><meta name="viewport" content="width=device-width,initial-scale=1">
<style>body{font-family:sans-serif;max-width:420px;margin:20px;background:#151a20;color:#eee}button,input{width:100%;margin:8px 0;font-size:18px}button{padding:12px}#arm{background:#c33;color:white}label{display:block}</style></head><body>
<h2>Quad setup control</h2><p id="status">Connect, keep propellers OFF, then arm.</p>
<button id="arm" onclick="toggleArm()">ARM</button>
<label>Throttle <output id="tv">0</output>%<input id="t" type="range" min="0" max="25" value="0" oninput="send()"></label>
<label>Roll <output id="rv">0</output><input id="r" type="range" min="-15" max="15" value="0" oninput="send()"></label>
<label>Pitch <output id="pv">0</output><input id="p" type="range" min="-15" max="15" value="0" oninput="send()"></label>
<label>Yaw <output id="yv">0</output><input id="y" type="range" min="-15" max="15" value="0" oninput="send()"></label>
<script>
let isArmed=false; const g=id=>document.getElementById(id);
function send(){['t','r','p','y'].forEach(x=>g(x+'v').textContent=g(x).value);fetch(`/set?t=${g('t').value}&r=${g('r').value}&p=${g('p').value}&y=${g('y').value}`).catch(()=>{});}
function toggleArm(){isArmed=!isArmed; if(!isArmed)g('t').value=0; fetch('/arm?state='+(isArmed?1:0)).then(()=>{g('arm').textContent=isArmed?'DISARM':'ARM';g('status').textContent=isArmed?'Armed: keep sending controls.':'Disarmed.';send();});}
setInterval(()=>{if(isArmed)send();},150); send();
</script></body></html>
)HTML";

void writeMotors(int fl, int fr, int rr, int rl) {
  escFL.writeMicroseconds(constrain(fl, ESC_STOP_US, ESC_MAX_US));
  escFR.writeMicroseconds(constrain(fr, ESC_STOP_US, ESC_MAX_US));
  escRR.writeMicroseconds(constrain(rr, ESC_STOP_US, ESC_MAX_US));
  escRL.writeMicroseconds(constrain(rl, ESC_STOP_US, ESC_MAX_US));
}

void stopMotors() { writeMotors(ESC_STOP_US, ESC_STOP_US, ESC_STOP_US, ESC_STOP_US); }

bool readImu() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom((int)MPU_ADDR, 14) != 14) return false;
  int16_t ax = (Wire.read() << 8) | Wire.read();
  int16_t ay = (Wire.read() << 8) | Wire.read();
  int16_t az = (Wire.read() << 8) | Wire.read();
  Wire.read(); Wire.read();
  int16_t gx = (Wire.read() << 8) | Wire.read();
  int16_t gy = (Wire.read() << 8) | Wire.read();
  Wire.read(); Wire.read();

  uint32_t nowUs = micros();
  float dt = (lastImuUs == 0) ? 0.004f : (nowUs - lastImuUs) / 1000000.0f;
  lastImuUs = nowUs;
  dt = constrain(dt, 0.001f, 0.02f);
  float accRoll = atan2f((float)ay, (float)az) * 180.0f / PI;
  float accPitch = atan2f(-(float)ax, sqrtf((float)ay * ay + (float)az * az)) * 180.0f / PI;
  rollDeg = 0.98f * (rollDeg + (gx / 131.0f) * dt) + 0.02f * accRoll;
  pitchDeg = 0.98f * (pitchDeg + (gy / 131.0f) * dt) + 0.02f * accPitch;
  return true;
}

void setupMpu() {
  Wire.beginTransmission(MPU_ADDR); Wire.write(0x6B); Wire.write(0x00); Wire.endTransmission(true);
  delay(100);
  // Start from the accelerometer tilt while the airframe is still and level.
  readImu();
}

void handleSet() {
  throttleCommand = constrain(server.arg("t").toFloat(), 0.0f, 25.0f);
  rollCommand = constrain(server.arg("r").toFloat(), -15.0f, 15.0f);
  pitchCommand = constrain(server.arg("p").toFloat(), -15.0f, 15.0f);
  yawCommand = constrain(server.arg("y").toFloat(), -15.0f, 15.0f);
  lastCommandMs = millis();
  server.send(200, "text/plain", "ok");
}

void setup() {
  Serial.begin(115200);
  escFL.setPeriodHertz(50); escFR.setPeriodHertz(50); escRR.setPeriodHertz(50); escRL.setPeriodHertz(50);
  escFL.attach(ESC_FL_PIN, 1000, 2000); escFR.attach(ESC_FR_PIN, 1000, 2000);
  escRR.attach(ESC_RR_PIN, 1000, 2000); escRL.attach(ESC_RL_PIN, 1000, 2000);
  stopMotors();
  Wire.begin(MPU_SDA_PIN, MPU_SCL_PIN);
  setupMpu();
  WiFi.mode(WIFI_AP);
  WiFi.softAP(AP_NAME, AP_PASSWORD);
  server.on("/", HTTP_GET, [](){ server.send_P(200, "text/html", INDEX_HTML); });
  server.on("/set", HTTP_GET, handleSet);
  server.on("/arm", HTTP_GET, [](){
    armed = server.arg("state") == "1";
    throttleCommand = 0.0f;
    lastCommandMs = millis();
    if (!armed) stopMotors();
    server.send(200, "text/plain", armed ? "armed" : "disarmed");
  });
  server.begin();
  Serial.println(WiFi.softAPIP());
}

void loop() {
  server.handleClient();
  bool imuOk = readImu();
  if (!armed || !imuOk || (millis() - lastCommandMs > COMMAND_TIMEOUT_MS) || fabsf(rollDeg) > MAX_TILT_DEG || fabsf(pitchDeg) > MAX_TILT_DEG) {
    armed = false;
    throttleCommand = 0.0f;
    stopMotors();
    return;
  }

  float base = ESC_STOP_US + throttleCommand * 20.0f;
  float rollCorrection = (rollCommand - rollDeg) * P_GAIN;
  float pitchCorrection = (pitchCommand - pitchDeg) * P_GAIN;
  float yawMix = yawCommand * 1.5f;
  // X frame: FL and RR rotate one way; FR and RL rotate the other way.
  writeMotors((int)(base + pitchCorrection - rollCorrection - yawMix),
              (int)(base + pitchCorrection + rollCorrection + yawMix),
              (int)(base - pitchCorrection + rollCorrection - yawMix),
              (int)(base - pitchCorrection - rollCorrection + yawMix));
}