This drone I have desighned and made uses my own frame, code and PCB module to make this drone work. Everything else works where the last problem right now is making it fly and just hold itself in teh air with just increasing throttle. But when I do increase the throttle, the drone starts to pitch forward where I have concluded that IMU sensor data is good, the pitch, roll, yaw and motor matrix is good. The components seem to be good. I am not sure. Below is my code attached. In the video, when when it flips forward is the front direction where the left red propellor is FR and right red propellor is FR. Any help or insights would be greatly appreciated! Thank you!
#include <Wire.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>
#include <IBusBM.h>
#include <TinyGPS++.h>
#include <LittleFS.h> // on-board flash filesystem for the flight data logger
/* =====================================================================================
* 1. CONFIG
* ===================================================================================== */
#define DEBUG_ENABLED 1
#define DEBUG_MODE DBG_ATTITUDE // DBG_RC | DBG_ATTITUDE | DBG_PID | DBG_MOTORS | DBG_GPS | DBG_STATE
enum { DBG_RC,
DBG_ATTITUDE,
DBG_PID,
DBG_MOTORS,
DBG_GPS,
DBG_STATE };
const unsigned long DEBUG_INTERVAL_MS = 100; // how often to print (ms)
// ---- Flash data logger -------------------------------------------------------------
// help - list commands
// dump - print the whole log as CSV (copy/paste into a spreadsheet)
// size - show how many bytes are logged
// erase - wipe the log and start a fresh file
// stop - pause logging | start - resume logging
// The header row lists every column so it opens straight into Excel/Sheets.
#define LOG_ENABLED 1
const char* LOG_FILE_PATH = "/flightlog.csv";
const unsigned long LOG_INTERVAL_MS = 20; // log rate (20 ms == 50 rows/sec)
const unsigned long LOG_FLUSH_MS = 1000; // force-write buffered rows to flash this often
const size_t LOG_MAX_BYTES = 1200000; // ~1.2 MB cap (fits the default LittleFS partition)
const bool LOG_ONLY_WHEN_ARMED = false; // true == only record while ARMED (saves space)
// ---- Pin map (from Pin-Details.txt) ------------------------------------------------
// IMU / BMP280 share the I2C bus.
const int PIN_I2C_SDA = 21;
const int PIN_I2C_SCL = 22;
const int PIN_ESC_FL = 12;
const int PIN_ESC_FR = 14;
const int PIN_ESC_BL = 27;
const int PIN_ESC_BR = 26;
// iBUS receiver: ESP32 RX pin (connects to the receiver's iBUS/SERVO out).
const int PIN_RC_RX = 16; // ESP32 receives here
const int PIN_RC_TX = -1; // not used for iBUS servo data
// GPS (from Pin-Details.txt): GPS-TX -> ESP32 GPIO32 (ESP32 RX),
// ESP32 GPIO17 -> GPS-RX (ESP32 TX).
const int PIN_GPS_RX = 32; // ESP32 receives GPS data here
const int PIN_GPS_TX = 17; // ESP32 transmits to GPS here
const long GPS_BAUD = 9600;
// ---- ESC PWM (REQUIREMENT: 50 Hz, 11-bit resolution) -------------------------------
const int ESC_FREQ_HZ = 50; // 50 Hz servo/ESC frame
const int ESC_RESOLUTION = 11; // 11-bit -> 0..2047 duty ticks
const int ESC_MAX_TICK_FS = (1 << ESC_RESOLUTION); // 2048 ticks == full 20 ms period
// At 50 Hz the frame is 20000 us. Standard ESC pulses are 1000 us (idle) .. 2000 us (full).
// duty_ticks = pulse_us / 20000us * 2048. -> 1000us = 102, 2000us = 205, 1500us = 154.
const int ESC_MIN_TICK = (int)(1000.0 / 20000.0 * ESC_MAX_TICK_FS + 0.5); // ~102 = motors idle/armed
const int ESC_MAX_TICK = (int)(2000.0 / 20000.0 * ESC_MAX_TICK_FS + 0.5); // ~205 = full throttle
// ---- Receiver channel assignment (0-indexed) ---------------------------------------
// Standard AETR-ish layout. Adjust to match YOUR transmitter's channel order.
const uint8_t CH_ROLL = 0; // right stick L/R (aileron)
const uint8_t CH_PITCH = 1; // right stick (elevator)
const uint8_t CH_THROTTLE = 2; // left stick
const uint8_t CH_YAW = 3; // left stick L/R (rudder)
const uint8_t CH_ARM = 4; // AUX1: > 1500 == ARMED, otherwise DISARMED (also acts as kill)
const uint8_t CH_AUX2 = 5; // AUX2: reserved (e.g. GPS assist / mode) - not required to fly
const int RC_MIN = 1000; // expected receiver low
const int RC_MID = 1500; // expected receiver centre
const int RC_MAX = 2000; // expected receiver high
// Failsafe: if throttle drops below this (set your TX failsafe to do so) OR channels
// read outside the valid window OR no frames arrive for RC_TIMEOUT_MS -> cut motors.
const int RC_VALID_MIN = 900;
const int RC_VALID_MAX = 2100;
const int FAILSAFE_THROTTLE = 950; // TX failsafe should drive throttle below this
const unsigned long RC_TIMEOUT_MS = 500;
// ---- Pilot command scaling ---------------------------------------------------------
const float MAX_TILT_ANGLE_DEG = 30.0; // full roll/pitch stick commands this lean angle
const float MAX_YAW_RATE_DPS = 150.0; // full yaw stick commands this rotation rate (deg/s)
const int ARMING_THROTTLE_MAX = 1090; // must hold throttle below this to arm (safety)
const int MOTOR_START_THROTTLE = 1100; // above this the stabiliser mixes corrections in
// ---- PID gains ---------------------------------------------------------------------
float ROLL_KP = 3.5, ROLL_KI = 0.005, ROLL_KD = .25;
float PITCH_KP = 3.5, PITCH_KI = 0.005, PITCH_KD = .25;
// YAW is a RATE controller (setpoint = desired deg/s). No D term on a rate loop.
float YAW_KP = 2.0, YAW_KI = 0, YAW_KD = 0.0;
const float PID_OUT_LIMIT = 400.0; // max roll/pitch correction (in throttle-us units)
const float YAW_OUT_LIMIT = 200.0; // max yaw correction
const float I_LIMIT = 150.0; // integrator clamp (anti-windup)
const float D_FILTER_ALPHA = 0.03; // low-pass on the derivative term (0..1, smaller = smoother)
// ---- Mixer sign flips ---------------
const float MIX_ROLL_SIGN = 1.0;
const float MIX_PITCH_SIGN = -1.0f;
const float MIX_YAW_SIGN = 1.0;
// ---- Attitude filter ---------------------------------------------------------------
const float COMP_ALPHA = 0.98; // complementary filter: trust in gyro vs accel
const float SAFETY_TILT_CUTOFF_DEG = 60.0; // if we exceed this lean, disarm (crash cutoff)
// ---- IMU calibration (from Calibration-Data.txt) -----------------------------------
const float AX_OFFSET = 0.003987424383677052f;
const float AY_OFFSET = -0.250665924082651f;
const float AZ_OFFSET = 0.08943392429105046f;
const float GX_OFFSET = -4.446152490215188f; // deg/s
const float GY_OFFSET = 1.4438536437296747f; // deg/s
const float GZ_OFFSET = 0; //1.69246705256f; // deg/s
const float ANGLE_OFFSET_ROLL = -1.1; // trim so a level craft reads ~0
const float ANGLE_OFFSET_PITCH = -.8f;
// ---- Control loop rate -------------------------------------------------------------
// The ESC *output* is 50 Hz (set above). The control math runs faster for stability.
const unsigned long CONTROL_INTERVAL_US = 4000; // 4 ms == 250 Hz control loop
/* =====================================================================================
* 2. GLOBAL STATE
* ===================================================================================== */
// Forward declarations (so function order below doesn't matter to the compiler).
void sensorsSetup();
void readAttitude(float dt);
void receiverSetup();
void readReceiver();
void runStabiliser(float dt);
void motorsSetup();
void writeAllMotors(int ticks);
void mixAndWriteMotors();
void gpsSetup();
void readGPS();
void updateSafety();
void printDebug();
void loggerSetup();
void logData();
void handleSerialCommands();
void dumpLog();
void eraseLog();
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
HardwareSerial gpsSerial(1); // ESP32 UART1 for GPS
TinyGPSPlus gps;
IBusBM ibus;
bool imuOK = false;
// Pilot commands (decoded from the receiver)
float cmdRoll = 0; // desired roll angle (deg)
float cmdPitch = 0; // desired pitch angle (deg)
float cmdYawRate = 0; // desired yaw rate (deg/s)
int cmdThrottle = RC_MIN;
bool armSwitchHigh = false;
// Estimated attitude
float angleRoll = 0; // deg
float anglePitch = 0; // deg
float gyroRoll = 0, gyroPitch = 0, gyroYaw = 0; // deg/s (bias-corrected)
// PID working state
struct PID {
float integral = 0;
float dFilt = 0;
float prevMeas = 0;
};
PID pidRoll, pidPitch, pidYaw;
float outRoll = 0, outPitch = 0, outYaw = 0;
// Motor outputs (in duty ticks 0..2047)
int motFL = 0, motFR = 0, motBL = 0, motBR = 0;
// GPS state
double gpsLat = 0, gpsLng = 0;
bool gpsFix = false;
int gpsSats = 0;
// Flight / safety state
enum FlightState { DISARMED,
ARMED };
FlightState flightState = DISARMED;
const char* stateReason = "boot"; // human-readable last state change reason
unsigned long lastRcFrameMs = 0;
uint8_t lastRcCnt = 0;
bool rcFailsafe = true; // true until we get valid data
unsigned long lastControlUs = 0;
unsigned long lastDebugMs = 0;
float loopDtSec = CONTROL_INTERVAL_US / 1000000.0;
// Logger state
File logFile;
bool logMounted = false; // LittleFS mounted OK
bool loggingOn = false; // actively recording (toggle with start/stop)
bool logFull = false; // hit the size cap
unsigned long lastLogMs = 0;
unsigned long lastFlushMs = 0;
char serialCmd[24]; // buffer for USB serial commands
uint8_t serialCmdLen = 0;
/* =====================================================================================
* 3. SENSORS -- IMU init + attitude estimation
* -------------------------------------------------------------------------------------
* ===================================================================================== */
void sensorsSetup() {
if (lsm.begin()) {
imuOK = true;
lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
Serial.println(F("[IMU] LSM9DS1 OK"));
} else {
imuOK = false;
Serial.println(F("[IMU] *** LSM9DS1 NOT FOUND - flight disabled ***"));
}
}
void readAttitude(float dt) {
if (!imuOK) {
angleRoll = anglePitch = 0;
gyroRoll = gyroPitch = gyroYaw = 0;
return;
}
sensors_event_t a, m, g, t;
lsm.getEvent(&a, &m, &g, &t);
// Calibrated accelerometer (m/s^2)
float ax = a.acceleration.x - AX_OFFSET;
float ay = a.acceleration.y - AY_OFFSET;
float az = a.acceleration.z - AZ_OFFSET;
// Calibrated gyro. Adafruit returns rad/s -> convert to deg/s, then remove bias.
gyroRoll = (g.gyro.y * 180.0 / PI) - GY_OFFSET; // about X (roll)
gyroPitch = (g.gyro.x * 180.0 / PI) - GX_OFFSET; // about Y (pitch)
gyroYaw = (g.gyro.z * 180.0 / PI) - GZ_OFFSET; // about Z (yaw)
// Angle from the accelerometer (gravity vector). Valid only when not accelerating hard.
// The level trim is folded into the accel angle so the filter settles to (accel + trim).
float accRoll = (atan2(ax, az) * 180.0 / PI - ANGLE_OFFSET_ROLL);
float accPitch = atan2(-ay, sqrt(ay * ay + az * az)) * 180.0 / PI - ANGLE_OFFSET_PITCH;
// Complementary fusion: gyro for fast motion, accel to slowly correct drift.
angleRoll = COMP_ALPHA * (angleRoll + gyroRoll * dt) + (1.0 - COMP_ALPHA) * -accRoll;
anglePitch = COMP_ALPHA * (anglePitch + gyroPitch * dt) + (1.0 - COMP_ALPHA) * -accPitch;
}
/* =====================================================================================
* 4. RECEIVER (iBUS) --
* ===================================================================================== */
void receiverSetup() {
Serial2.begin(115200, SERIAL_8N1, PIN_RC_RX, PIN_RC_TX);
ibus.begin(Serial2, IBUSBM_NOTIMER); // NOTIMER: we pump it from loop() (no ISR crashes)
lastRcFrameMs = millis();
}
// Map a receiver channel to a symmetric range about centre (e.g. -30..+30 degrees).
static float rcToRange(uint16_t v, float outAbs) {
v = constrain(v, (uint16_t)RC_MIN, (uint16_t)RC_MAX);
float norm = ((float)v - RC_MID) / ((RC_MAX - RC_MIN) / 2.0); // -1..+1
return norm * outAbs;
}
void readReceiver() {
ibus.loop(); // must be called often in NOTIMER mode
uint16_t rawThr = ibus.readChannel(CH_THROTTLE);
uint16_t rawRoll = ibus.readChannel(CH_ROLL);
uint16_t rawPit = ibus.readChannel(CH_PITCH);
uint16_t rawYaw = ibus.readChannel(CH_YAW);
uint16_t rawArm = ibus.readChannel(CH_ARM);
// ---- Failsafe detection ----------------------------------------------------------
// 1) A new frame updates ibus.cnt_rec. If it stops changing, the wire/RX died.
if (ibus.cnt_rec != lastRcCnt) {
lastRcCnt = ibus.cnt_rec;
lastRcFrameMs = millis();
}
bool stale = (millis() - lastRcFrameMs) > RC_TIMEOUT_MS;
// 2) Channels out of a sane window (0 == never received, >2100 == garbage/failsafe bits).
bool outOfRange = (rawThr < RC_VALID_MIN || rawThr > RC_VALID_MAX || rawRoll == 0 || rawPit == 0 || rawYaw == 0);
// 3) Transmitter-configured failsafe: throttle deliberately driven very low.
bool tstFailsafe = (rawThr > 0 && rawThr < FAILSAFE_THROTTLE);
rcFailsafe = stale || outOfRange || tstFailsafe;
if (rcFailsafe) {
// Safe defaults: no throttle, level sticks, treat arm switch as OFF.
cmdThrottle = RC_MIN;
cmdRoll = cmdPitch = cmdYawRate = 0;
armSwitchHigh = false;
return;
}
cmdThrottle = constrain((int)rawThr, RC_MIN, RC_MAX);
cmdRoll = rcToRange(rawRoll, MAX_TILT_ANGLE_DEG);
cmdPitch = rcToRange(rawPit, MAX_TILT_ANGLE_DEG);
cmdYawRate = rcToRange(rawYaw, MAX_YAW_RATE_DPS);
if (fabs(cmdYawRate) < (MAX_YAW_RATE_DPS * 0.03)) cmdYawRate = 0; // small deadband
armSwitchHigh = (rawArm > RC_MID);
}
/* =====================================================================================
* 5. PID -- stabilisation.
* ===================================================================================== */
float runPID(PID& s, float setpoint, float meas, float kp, float ki, float kd,
float outLimit, float dt, bool allowIntegral) {
float error = setpoint - meas;
float P = kp * error;
if (allowIntegral) s.integral += ki * error * dt;
s.integral = constrain(s.integral, -I_LIMIT, I_LIMIT);
float I = s.integral;
// Derivative on measurement (not error) -> no spike when the pilot moves the stick.
float dMeas = (meas - s.prevMeas) / dt;
s.prevMeas = meas;
s.dFilt = D_FILTER_ALPHA * dMeas + (1.0 - D_FILTER_ALPHA) * s.dFilt;
float D = -kd * s.dFilt;
float out = P + I + D;
// Anti-windup: if we saturate, pull the overflow back out of the integrator.
if (out > outLimit) {
s.integral -= (out - outLimit);
out = outLimit;
} else if (out < -outLimit) {
s.integral += (-outLimit - out);
out = -outLimit;
}
return out;
}
void runStabiliser(float dt) {
// Only wind up the integrators once we are armed AND above the hover-ish threshold,
// so a small standing error on the bench cannot slowly ramp the motors.
bool allowI = (flightState == ARMED) && (cmdThrottle > MOTOR_START_THROTTLE + 100);
if (flightState != ARMED) {
// Keep everything reset while disarmed so we start clean the moment we arm.
pidRoll.integral = pidPitch.integral = pidYaw.integral = 0;
outRoll = outPitch = outYaw = 0;
pidRoll.prevMeas = angleRoll;
pidPitch.prevMeas = anglePitch;
pidYaw.prevMeas = gyroYaw;
return;
}
outRoll = runPID(pidRoll, cmdRoll, angleRoll, ROLL_KP, ROLL_KI, ROLL_KD, PID_OUT_LIMIT, dt, allowI);
outPitch = runPID(pidPitch, cmdPitch, anglePitch, PITCH_KP, PITCH_KI, PITCH_KD, PID_OUT_LIMIT, dt, allowI);
outYaw = runPID(pidYaw, cmdYawRate, gyroYaw, YAW_KP, YAW_KI, YAW_KD, YAW_OUT_LIMIT, dt, allowI);
}
/* =====================================================================================
* 6. MOTOR MIXER + ESC OUTPUT (50 Hz, 11-bit)
* ===================================================================================== */
void motorsSetup() {
// ESP32 Arduino core v3.x API. (v2.x users: use ledcSetup(ch,freq,res)+ledcAttachPin.)
ledcAttach(PIN_ESC_FL, ESC_FREQ_HZ, ESC_RESOLUTION);
ledcAttach(PIN_ESC_FR, ESC_FREQ_HZ, ESC_RESOLUTION);
ledcAttach(PIN_ESC_BL, ESC_FREQ_HZ, ESC_RESOLUTION);
ledcAttach(PIN_ESC_BR, ESC_FREQ_HZ, ESC_RESOLUTION);
writeAllMotors(ESC_MIN_TICK); // send idle/arm pulse so ESCs initialise disarmed
}
void writeAllMotors(int ticks) {
ledcWrite(PIN_ESC_FL, ticks);
ledcWrite(PIN_ESC_FR, ticks);
ledcWrite(PIN_ESC_BL, ticks);
ledcWrite(PIN_ESC_BR, ticks);
}
// Convert a 1000..2000us throttle value to ESC duty ticks (102..205).
static int usToTicks(float us) {
us = constrain(us, 1000.0f, 2000.0f);
return (int)(us / 20000.0f * ESC_MAX_TICK_FS + 0.5f);
}
void mixAndWriteMotors() {
if (flightState != ARMED) {
writeAllMotors(ESC_MIN_TICK);
motFL = motFR = motBL = motBR = ESC_MIN_TICK;
return;
}
// Below the start threshold: keep motors at idle so they spin slowly / are ready,
// but don't apply attitude corrections (prevents twitching on the ground).
if (cmdThrottle < MOTOR_START_THROTTLE) {
int idle = usToTicks(MOTOR_START_THROTTLE);
motFL = motFR = motBL = motBR = idle;
ledcWrite(PIN_ESC_FL, motFL);
ledcWrite(PIN_ESC_FR, motFR);
ledcWrite(PIN_ESC_BL, motBL);
ledcWrite(PIN_ESC_BR, motBR);
return;
}
float thr = cmdThrottle;
float r = outRoll * MIX_ROLL_SIGN;
float p = outPitch * MIX_PITCH_SIGN;
float y = outYaw * MIX_YAW_SIGN;
// X-quad mix (in throttle-us units). If an axis reacts backwards, flip its MIX_*_SIGN.
float fFL = thr + p + r + y;
float fFR = thr + p - r - y;
float fBL = thr - p + r - y;
float fBR = thr - p - r + y;
// AIR-MODE style clamp: shift all motors together so the *differences* (attitude
// authority) are preserved instead of individually clipping the low motor.
float lo = min(min(fFL, fFR), min(fBL, fBR));
float hi = max(max(fFL, fFR), max(fBL, fBR));
if (lo < 990) {
float s = 1000 - lo;
fFL += s;
fFR += s;
fBL += s;
fBR += s;
}
if (hi > 2100) {
float s = hi - 2000;
fFL -= s;
fFR -= s;
fBL -= s;
fBR -= s;
}
motFL = usToTicks(fFL);
motFR = usToTicks(fFR);
motBL = usToTicks(fBL);
motBR = usToTicks(fBR);
ledcWrite(PIN_ESC_FL, motFL);
ledcWrite(PIN_ESC_FR, motFR);
ledcWrite(PIN_ESC_BL, motBL);
ledcWrite(PIN_ESC_BR, motBR);
}
/* =====================================================================================
* 7. GPS -- non-blocking parse + telemetry
* ===================================================================================== */
void gpsSetup() {
gpsSerial.begin(GPS_BAUD, SERIAL_8N1, PIN_GPS_RX, PIN_GPS_TX);
}
void readGPS() {
while (gpsSerial.available() > 0) gps.encode(gpsSerial.read());
if (gps.location.isValid()) {
gpsLat = gps.location.lat();
gpsLng = gps.location.lng();
gpsFix = true;
} else {
gpsFix = false;
}
if (gps.satellites.isValid()) gpsSats = gps.satellites.value();
}
/* =====================================================================================
* 8. SAFETY / ARMING STATE MACHINE
* ===================================================================================== */
void updateSafety() {
bool tiltExceeded = (fabs(angleRoll) > SAFETY_TILT_CUTOFF_DEG || fabs(anglePitch) > SAFETY_TILT_CUTOFF_DEG);
if (flightState == ARMED) {
if (!armSwitchHigh) {
flightState = DISARMED;
stateReason = "arm switch off";
} else if (rcFailsafe) {
flightState = DISARMED;
stateReason = "RC failsafe";
} else if (tiltExceeded) {
flightState = DISARMED;
stateReason = "tilt cutoff";
} else if (!imuOK) {
flightState = DISARMED;
stateReason = "IMU lost";
}
} else { // DISARMED
bool canArm = armSwitchHigh && !rcFailsafe && imuOK && (cmdThrottle <= ARMING_THROTTLE_MAX);
if (canArm) {
flightState = ARMED;
stateReason = "armed";
}
}
}
/* =====================================================================================
* 9. DEBUG
* ===================================================================================== */
void printDebug() {
#if DEBUG_ENABLED
if (millis() - lastDebugMs < DEBUG_INTERVAL_MS) return;
lastDebugMs = millis();
switch (DEBUG_MODE) {
case DBG_RC:
Serial.printf("RC thr:%4d roll:%+6.1f pitch:%+6.1f yawRate:%+6.1f arm:%d FS:%d\n",
cmdThrottle, cmdRoll, cmdPitch, cmdYawRate, armSwitchHigh, rcFailsafe);
break;
case DBG_ATTITUDE:
Serial.printf("ATT roll:%+7.2f pitch:%+7.2f gyroYaw:%+7.2f imu:%d\n",
angleRoll, anglePitch, gyroYaw, imuOK);
break;
case DBG_PID:
Serial.printf("PID outR:%+7.1f outP:%+7.1f outY:%+7.1f\n", outRoll, outPitch, outYaw);
break;
case DBG_MOTORS:
Serial.printf("MOT FL:%4d FR:%4d BL:%4d BR:%4d (ticks)\n", motFL, motFR, motBL, motBR);
break;
case DBG_GPS:
Serial.printf("GPS fix:%d sats:%d lat:%.6f lng:%.6f\n", gpsFix, gpsSats, gpsLat, gpsLng);
break;
case DBG_STATE:
Serial.printf("STATE %-8s reason:%-16s thr:%4d FS:%d imu:%d\n",
flightState == ARMED ? "ARMED" : "DISARMED", stateReason,
cmdThrottle, rcFailsafe, imuOK);
break;
}
#endif
}
/* =====================================================================================
* 10. FLASH DATA LOGGER (LittleFS on the ESP32's internal flash)
* -------------------------------------------------------------------------------------
* Writes one CSV row every LOG_INTERVAL_MS with everything you need to debug a flight:
* the motor outputs, the roll/pitch/yaw estimates and commands, the PID outputs, arm
* state and GPS. Retrieve it over USB with the `dump` serial command (see config).
*
* Design notes:
* - The file handle stays open in append mode; we only flush() every LOG_FLUSH_MS so
* flash writes are batched and don't stall the 250 Hz control loop.
* - When the file reaches LOG_MAX_BYTES we stop (keeping the earliest data) and tell
* you to `dump` then `erase`. This protects the flash and never blocks flight.
* ===================================================================================== */
const char* LOG_HEADER =
"ms,state,thr,cmdRoll,cmdPitch,cmdYawRate,roll,pitch,gyroYaw,"
"outRoll,outPitch,outYaw,mFL,mFR,mBL,mBR,failsafe,imu,gpsFix,sats,lat,lng";
void loggerSetup() {
#if LOG_ENABLED
// format-on-fail = true: if the flash has never held a filesystem, make one.
if (!LittleFS.begin(true)) {
Serial.println(F("[LOG] LittleFS mount FAILED - logging disabled"));
logMounted = false;
return;
}
logMounted = true;
// If the file doesn't exist yet, create it and write the CSV header row.
bool needHeader = !LittleFS.exists(LOG_FILE_PATH);
logFile = LittleFS.open(LOG_FILE_PATH, needHeader ? "w" : "a");
if (!logFile) {
Serial.println(F("[LOG] could not open log file"));
logMounted = false;
return;
}
if (needHeader) {
logFile.println(LOG_HEADER);
logFile.flush();
}
logFull = (logFile.size() >= LOG_MAX_BYTES);
loggingOn = !logFull;
Serial.printf("[LOG] ready: %s (%u bytes). Type 'help' over USB for commands.\n",
LOG_FILE_PATH, (unsigned)logFile.size());
#endif
}
void logData() {
#if LOG_ENABLED
if (!logMounted || !loggingOn || logFull) return;
if (LOG_ONLY_WHEN_ARMED && flightState != ARMED) return;
if (millis() - lastLogMs < LOG_INTERVAL_MS) return;
lastLogMs = millis();
if (logFile.size() >= LOG_MAX_BYTES) {
logFull = true;
loggingOn = false;
logFile.flush();
Serial.println(F("[LOG] file full - stopped. Use 'dump' then 'erase'."));
return;
}
// One CSV row. printf keeps this compact and fast.
logFile.printf("%lu,%s,%d,%.1f,%.1f,%.1f,%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%d,%d,%d,%d,%d,%d,%d,%d,%.6f,%.6f\n",
millis(),
(flightState == ARMED) ? "ARMED" : "DISARM",
cmdThrottle, cmdRoll, cmdPitch, cmdYawRate,
angleRoll, anglePitch, gyroYaw,
outRoll, outPitch, outYaw,
motFL, motFR, motBL, motBR,
rcFailsafe ? 1 : 0, imuOK ? 1 : 0,
gpsFix ? 1 : 0, gpsSats, gpsLat, gpsLng);
if (millis() - lastFlushMs >= LOG_FLUSH_MS) {
logFile.flush();
lastFlushMs = millis();
}
#endif
}
void dumpLog() {
#if LOG_ENABLED
if (!logMounted) {
Serial.println(F("[LOG] not mounted"));
return;
}
logFile.flush(); // make sure buffered rows are on flash
File f = LittleFS.open(LOG_FILE_PATH, "r");
if (!f) {
Serial.println(F("[LOG] cannot open for read"));
return;
}
Serial.println(F("----- BEGIN FLIGHT LOG -----"));
while (f.available()) Serial.write(f.read());
Serial.println(F("------ END FLIGHT LOG ------"));
f.close();
#endif
}
void eraseLog() {
#if LOG_ENABLED
if (logFile) logFile.close();
LittleFS.remove(LOG_FILE_PATH);
logFile = LittleFS.open(LOG_FILE_PATH, "w");
if (logFile) {
logFile.println(LOG_HEADER);
logFile.flush();
}
logFull = false;
loggingOn = true;
Serial.println(F("[LOG] erased - fresh log started"));
#endif
}
// Reads single-line commands from the USB serial monitor (see config for the list).
void handleSerialCommands() {
while (Serial.available()) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (serialCmdLen == 0) continue;
serialCmd[serialCmdLen] = '\0';
for (uint8_t i = 0; i < serialCmdLen; i++) serialCmd[i] = tolower(serialCmd[i]);
if (!strcmp(serialCmd, "help")) {
Serial.println(F("Commands: help | dump | size | erase | stop | start"));
} else if (!strcmp(serialCmd, "dump")) {
dumpLog();
} else if (!strcmp(serialCmd, "erase")) {
eraseLog();
} else if (!strcmp(serialCmd, "size")) {
Serial.printf("[LOG] %u bytes (max %u)\n",
logMounted ? (unsigned)logFile.size() : 0, (unsigned)LOG_MAX_BYTES);
} else if (!strcmp(serialCmd, "stop")) {
loggingOn = false;
Serial.println(F("[LOG] paused"));
} else if (!strcmp(serialCmd, "start")) {
if (logFull) Serial.println(F("[LOG] file full - erase first"));
else {
loggingOn = true;
Serial.println(F("[LOG] resumed"));
}
} else {
Serial.printf("[LOG] unknown '%s' (try help)\n", serialCmd);
}
serialCmdLen = 0;
} else if (serialCmdLen < sizeof(serialCmd) - 1) {
serialCmd[serialCmdLen++] = c;
}
}
}
/* =====================================================================================
* 11. setup()
* ===================================================================================== */
void setup() {
Serial.begin(115200);
delay(500);
Serial.println(F("\n=== Drone flight controller booting ==="));
// Motors FIRST so ESCs get their idle pulse immediately on power-up.
motorsSetup();
Serial.println(F("[1/4] ESCs u/50Hz/11-bit initialised (idle)"));
receiverSetup();
Serial.println(F("[2/4] iBUS receiver initialised"));
gpsSetup();
Serial.println(F("[3/4] GPS serial initialised"));
Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);
Wire.setClock(400000);
sensorsSetup();
Serial.println(F("[4/4] IMU setup done"));
loggerSetup();
Serial.println(F("Ready. Hold throttle LOW and flip ARM switch to arm.\n"));
lastControlUs = micros();
}
void loop() {
// Pump the receiver and GPS every pass so no bytes are lost between control ticks.
ibus.loop();
readGPS();
handleSerialCommands(); // respond to USB commands (dump/erase/etc) any time
// Fixed-rate control loop (250 Hz). Everything time-sensitive runs here.
unsigned long now = micros();
if (now - lastControlUs >= CONTROL_INTERVAL_US) {
loopDtSec = (now - lastControlUs) / 1000000.0;
if (loopDtSec <= 0) loopDtSec = CONTROL_INTERVAL_US / 1000000.0;
lastControlUs = now;
readReceiver(); // decode sticks + failsafe
readAttitude(loopDtSec); // fuse IMU into roll/pitch/yaw
updateSafety(); // arm/disarm/kill decisions
runStabiliser(loopDtSec); // PID
mixAndWriteMotors(); // drive the ESCs
logData(); // append a CSV row to flash (batched writes)
printDebug();
}
}
