ESP32 Quadcopter Drone Controller

/*
 * Flix-compatible Quadcopter — ESP32 Pin Definitions
 *
 * Motor mapping (Flix convention):
 *   Motor 0 — Rear Left,   CCW, Prop B  → GPIO12
 *   Motor 1 — Rear Right,  CW,  Prop A  → GPIO13
 *   Motor 2 — Front Right, CCW, Prop B  → GPIO14
 *   Motor 3 — Front Left,  CW,  Prop A  → GPIO15
 *
 * IMU (MPU-6500, SPI mode):
 *   SCK  → GPIO18
 *   MOSI → GPIO23
 *   MISO → GPIO19
 *   CS   → GPIO5
 *
 * RC Receiver:
 *   SIGNAL (TX) → GPIO4
 *
 * NOTE: GPIO12, 15 are boot-strapping pins on ESP32.
 *   They are used as outputs here (motor PWM). All motor
 *   PWM outputs are LOW at boot, so no strapping conflict occurs
 *   in normal use. Verified consistent with Flix hardware design.
 *
 * NOTE: GPIO5 is also a boot-strapping pin (SPI CS).
 *   It is driven HIGH at boot by the internal pull-up, which is
 *   correct SPI CS idle state (deasserted). Safe to use.
 */

#include <Arduino.h>
#include <SPI.h>
#include <MPU6500_WE.h>

// ── Pin definitions ──────────────────────────────────────────────
#define MOTOR0_PIN    12   // Rear Left,   CCW, Prop B
#define MOTOR1_PIN    13   // Rear Right,  CW,  Prop A
#define MOTOR2_PIN    14   // Front Right, CCW, Prop B
#define MOTOR3_PIN    15   // Front Left,  CW,  Prop A

#define IMU_SCK       18
#define IMU_MOSI      23
#define IMU_MISO      19
#define IMU_CS         5

#define RC_SIGNAL_PIN  4

// ── LEDC (PWM) config ─────────────────────────────────────────────
#define PWM_FREQ      20000   // 20 kHz — above audible range
#define PWM_RES       8       // 8-bit: 0–255
#define CH_M0         0
#define CH_M1         1
#define CH_M2         2
#define CH_M3         3

// ── IMU ───────────────────────────────────────────────────────────

// Forward declarations
void setMotor(uint8_t channel, uint8_t throttle);
void setAllMotors(uint8_t throttle);

MPU6500_WE imu(IMU_CS);   // SPI chip-select pin

// ── Helpers ──────────────────────────────────────────────────────
void setMotor(uint8_t channel, uint8_t throttle) {
    ledcWrite(channel, throttle);
}

void setAllMotors(uint8_t throttle) {
    setMotor(CH_M0, throttle);
    setMotor(CH_M1, throttle);
    setMotor(CH_M2, throttle);
    setMotor(CH_M3, throttle);
}

// ── Setup ─────────────────────────────────────────────────────────
void setup() {
    Serial.begin(115200);
    Serial.println("[FLIX] Booting...");

    // Motors — initialise to zero throttle immediately
    ledcSetup(CH_M0, PWM_FREQ, PWM_RES);
    ledcSetup(CH_M1, PWM_FREQ, PWM_RES);
    ledcSetup(CH_M2, PWM_FREQ, PWM_RES);
    ledcSetup(CH_M3, PWM_FREQ, PWM_RES);
    ledcAttachPin(MOTOR0_PIN, CH_M0);
    ledcAttachPin(MOTOR1_PIN, CH_M1);
    ledcAttachPin(MOTOR2_PIN, CH_M2);
    ledcAttachPin(MOTOR3_PIN, CH_M3);
    setAllMotors(0);
    Serial.println("[FLIX] Motors initialised (all OFF)");

    // SPI bus for IMU
    SPI.begin(IMU_SCK, IMU_MISO, IMU_MOSI, IMU_CS);

    // IMU init
    if (!imu.init()) {
        Serial.println("[FLIX] ERROR: MPU-6500 not found — check SPI wiring!");
        while (1) { delay(500); }
    }
    imu.enableGyrDLPF();
    imu.setGyrDLPF(MPU6500_DLPF_6);
    imu.setSampleRateDivider(4);       // ~200 Hz
    imu.setGyrRange(MPU6500_GYRO_RANGE_500);
    imu.setAccRange(MPU6500_ACC_RANGE_4G);
    imu.enableAccDLPF(true);
    imu.setAccDLPF(MPU6500_DLPF_6);
    Serial.println("[FLIX] MPU-6500 IMU ready");

    // RC receiver pin
    pinMode(RC_SIGNAL_PIN, INPUT);
    Serial.println("[FLIX] RC signal pin ready (GPIO4)");

    Serial.println("[FLIX] Init complete. Replace this stub with Flix firmware.");
}

// ── Loop — stub (replace with Flix PID + RC loop) ─────────────────
void loop() {
    // Read IMU
    xyzFloat gyr = imu.getGyrValues();
    xyzFloat acc = imu.getGValues();

    Serial.printf("[IMU] Gyr: %.2f  %.2f  %.2f  |  Acc: %.2f  %.2f  %.2f\n",
                  gyr.x, gyr.y, gyr.z, acc.x, acc.y, acc.z);

    // ── Replace below with your Flix PID + RC input logic ──
    // Motor 0 (rear-left,   CCW): setMotor(CH_M0, throttle0);
    // Motor 1 (rear-right,  CW) : setMotor(CH_M1, throttle1);
    // Motor 2 (front-right, CCW): setMotor(CH_M2, throttle2);
    // Motor 3 (front-left,  CW) : setMotor(CH_M3, throttle3);
    setAllMotors(0);  // safe default: motors off

    delay(5);  // ~200 Hz loop rate placeholder
}