flight_code

#include <Wire.h>
#include <Adafruit_BMP280.h>
#include "driver/ledc.h"

// ========== HARDWARE CONFIGURATION ==========
// MPU9250 (IMU) - I2C0
#define MPU_ADDR 0x68
#define AK8963_ADDR 0x0C
#define I2C_SDA 21
#define I2C_SCL 22

// MPU9250 registers
#define PWR_MGMT_1 0x6B
#define SMPLRT_DIV 0x19
#define CONFIG_REG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_CONFIG2 0x1D
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define ACCEL_XOUT_H 0x3B
#define TEMP_OUT_H 0x41
#define GYRO_XOUT_H 0x43
#define USER_CTRL 0x6A
#define I2C_MST_CTRL 0x24
#define I2C_SLV0_ADDR 0x25
#define I2C_SLV0_REG 0x26
#define I2C_SLV0_CTRL 0x27
#define I2C_MST_STATUS 0x36
#define I2C_SLV0_DO 0x63
#define WHO_AM_I_MPU 0x75

// AK8963 registers
#define AK8963_WHO_AM_I 0x00
#define AK8963_ST1 0x02
#define AK8963_HXL 0x03
#define AK8963_CNTL1 0x0A
#define AK8963_ASAX 0x10

// Scale factors
float accelScale = 2.0f / 32768.0f; // g/LSB for ±2g
float gyroScale = 250.0f / 32768.0f; // dps/LSB for ±250 dps
float magAdj[3] = {1.0f, 1.0f, 1.0f}; // ASA adjustment from AK8963 fuse ROM
float magScale = 4912.0f / 32760.0f; // µT/LSB for 16-bit, 0x16 mode

// BMP280 (Barometer) - I2C1
#define I2C1_SDA 32
#define I2C1_SCL 33
TwoWire I2C_1 = TwoWire(1);
Adafruit_BMP280 bmp(&I2C_1);

// Control pins
#define ARM_SWITCH_PIN 12
#define LED_RED 26
#define LED_GREEN 27
#define CALIB_PIN 0

// PWM Configuration
const uint32_t PWM_FREQ_HZ = 50;
const uint8_t PWM_RES_BITS = 16;
const uint32_t PWM_MAX_DUTY = (1UL << PWM_RES_BITS) - 1;
const uint16_t MIN_US = 1000;
const uint16_t MAX_US = 2000;
const uint32_t PERIOD_US = 1000000UL / PWM_FREQ_HZ;

// ========== MOTOR COORDINATION FIX - RAISED THROTTLE THRESHOLDS ==========
const uint16_t THROTTLE_DEADBAND = 1200;      // Raised from 1100 to 1200µs
const uint16_t MOTOR_IDLE_SPEED = 1200;       // Minimum motor speed when armed
const uint16_t MOTOR_MIN_THROTTLE = 1200;    // Minimum throttle for motor calculations

// ========== STRUCTURES ==========
struct RcChannel {
    uint8_t pin;
    volatile uint32_t riseTime;
    volatile uint16_t pulseWidth;
    volatile bool newValue;
    uint16_t minVal;
    uint16_t maxVal;
    uint16_t centerVal;
    uint32_t lastUpdate;
};

// RC CALIBRATION VALUES UPDATED FROM YOUR CALIBRATION
RcChannel channels[6] = {
    // pin, riseTime, pulseWidth (center), newValue, minVal, maxVal, centerVal, lastUpdate
    {34, 0, 1575, false, 1160, 1990, 1575, 0}, // CH1 - Roll
    {35, 0, 1407, false, 1012, 1803, 1407, 0}, // CH2 - Pitch
    {4, 0, 1452, false, 1044, 1861, 1452, 0}, // CH3 - Throttle
    {2, 0, 1455, false, 1063, 1848, 1455, 0}, // CH4 - Yaw
    {15, 0, 1188, false, 1186, 1191, 1188, 0}, // CH5 - Mode
    {14, 0, 1013, false, 1011, 1016, 1013, 0} // CH6 - Aux
};

struct EscOut {
    int pin;
    ledc_channel_t channel;
    ledc_timer_t timer;
    ledc_mode_t mode;
};

EscOut escOuts[6] = {
    {5, LEDC_CHANNEL_0, LEDC_TIMER_0, LEDC_LOW_SPEED_MODE},
    {18, LEDC_CHANNEL_1, LEDC_TIMER_1, LEDC_LOW_SPEED_MODE},
    {23, LEDC_CHANNEL_2, LEDC_TIMER_2, LEDC_LOW_SPEED_MODE},
    {27, LEDC_CHANNEL_3, LEDC_TIMER_3, LEDC_LOW_SPEED_MODE}, // GPIO27 for consistency
    {19, LEDC_CHANNEL_4, LEDC_TIMER_0, LEDC_HIGH_SPEED_MODE},
    {25, LEDC_CHANNEL_5, LEDC_TIMER_1, LEDC_HIGH_SPEED_MODE}
};

enum FlightMode {
    STABILIZE = 0,
    ALTITUDE_HOLD = 1,
    ACRO = 2
};

struct FlightState {
    bool armed = false;
    FlightMode mode = STABILIZE;
    float roll = 0, pitch = 0, yaw = 0;
    float rollRate = 0, pitchRate = 0, yawRate = 0;
    float altitude = 0;
    float targetAltitude = 0;
    uint16_t throttle = 1000;
    uint32_t lastUpdate = 0;
    uint32_t lastRcUpdate = 0;
    bool failsafe = false;
} flight;

struct PIDController {
    float kp, ki, kd;
    float integral = 0;
    float lastError = 0;
    float lastTime = 0;
    float integralMax = 100;

    // Updated throttle threshold for I-term reset
    float calculate(float setpoint, float measured, uint16_t throttle) {
        float now = millis();
        float dt = (now - lastTime) / 1000.0;
        if (dt <= 0 || dt > 0.1) dt = 0.01;

        float error = setpoint - measured;
        integral += error * dt;

        // If throttle is below new deadband, reset the integral term
        if (throttle < THROTTLE_DEADBAND) {
            integral = 0;
        }

        integral = constrain(integral, -integralMax, integralMax);
        float derivative = (error - lastError) / dt;
        float output = kp * error + ki * integral + kd * derivative;

        lastError = error;
        lastTime = now;

        return output;
    }

    // Original calculate function (for Yaw and Altitude which don't need the throttle check)
    float calculate(float setpoint, float measured) {
        return calculate(setpoint, measured, 1500);
    }

    void reset() {
        integral = 0;
        lastError = 0;
        lastTime = millis();
    }
};

// Conservative PID gains for safer testing
PIDController rollPID = {0.5, 0.02, 0.1};
PIDController pitchPID = {0.5, 0.02, 0.1};
PIDController yawPID = {1.0, 0.05, 0.05};
PIDController rollRatePID = {0.8, 0.2, 0.01};
PIDController pitchRatePID = {0.8, 0.2, 0.01};
PIDController yawRatePID = {1.0, 0.2, 0.01};
PIDController altitudePID = {1.5, 0.1, 0.8};

// Sensor variables
float ax, ay, az, gx, gy, gz, tempC, mx, my, mz;

// SENSOR OFFSETS UPDATED FROM YOUR CALIBRATION
float axOffset = -0.0492, ayOffset = 0.0747, azOffset = 0.0326;
float gxOffset = 2.7156, gyOffset = 1.3944, gzOffset = -0.1386;

// ========== SENSOR FUNCTIONS ==========
void i2cWriteByte(uint8_t addr, uint8_t reg, uint8_t data) {
    Wire.beginTransmission(addr);
    Wire.write(reg);
    Wire.write(data);
    Wire.endTransmission(true);
}

void i2cReadBytes(uint8_t addr, uint8_t reg, uint8_t count, uint8_t* dest) {
    Wire.beginTransmission(addr);
    Wire.write(reg);
    Wire.endTransmission(false);
    Wire.requestFrom((int)addr, (int)count, (int)true);
    for (int i = 0; i < count && Wire.available(); i++) {
        dest[i] = Wire.read();
    }
}

int16_t read16(uint8_t addr, uint8_t regHigh) {
    uint8_t buf[2];
    i2cReadBytes(addr, regHigh, 2, buf);
    return (int16_t)((buf[0] << 8) | buf[1]);
}

void ak8963WriteReg(uint8_t reg, uint8_t val) {
    i2cWriteByte(MPU_ADDR, I2C_SLV0_ADDR, AK8963_ADDR);
    i2cWriteByte(MPU_ADDR, I2C_SLV0_REG, reg);
    i2cWriteByte(MPU_ADDR, I2C_SLV0_DO, val);
    i2cWriteByte(MPU_ADDR, I2C_SLV0_CTRL, 0x81);
    delay(10);
}

void mpuSetupI2CMaster() {
    i2cWriteByte(MPU_ADDR, USER_CTRL, 0x20);
    i2cWriteByte(MPU_ADDR, I2C_MST_CTRL, 0x0D);
    delay(10);
}

void ak8963Init() {
    ak8963WriteReg(AK8963_CNTL1, 0x00);
    delay(20);
    ak8963WriteReg(AK8963_CNTL1, 0x0F);
    delay(20);

    i2cWriteByte(MPU_ADDR, I2C_SLV0_ADDR, 0x80 | AK8963_ADDR);
    i2cWriteByte(MPU_ADDR, I2C_SLV0_REG, AK8963_ASAX);
    i2cWriteByte(MPU_ADDR, I2C_SLV0_CTRL, 0x83);
    delay(20);

    uint8_t ext[3] = {0};
    i2cReadBytes(MPU_ADDR, 0x49, 3, ext);

    for (int i = 0; i < 3; i++) {
        magAdj[i] = ((float)ext[i] - 128.0f) / 256.0f + 1.0f;
    }

    ak8963WriteReg(AK8963_CNTL1, 0x00);
    delay(20);
    ak8963WriteReg(AK8963_CNTL1, 0x16);
    delay(20);

    i2cWriteByte(MPU_ADDR, I2C_SLV0_ADDR, 0x80 | AK8963_ADDR);
    i2cWriteByte(MPU_ADDR, I2C_SLV0_REG, AK8963_HXL);
    i2cWriteByte(MPU_ADDR, I2C_SLV0_CTRL, 0x87);
}

bool mpuInit() {
    i2cWriteByte(MPU_ADDR, PWR_MGMT_1, 0x01);
    delay(100);

    i2cWriteByte(MPU_ADDR, CONFIG_REG, 0x03);
    i2cWriteByte(MPU_ADDR, SMPLRT_DIV, 9);
    i2cWriteByte(MPU_ADDR, GYRO_CONFIG, 0x00);
    i2cWriteByte(MPU_ADDR, ACCEL_CONFIG, 0x00);
    i2cWriteByte(MPU_ADDR, ACCEL_CONFIG2, 0x03);

    mpuSetupI2CMaster();
    ak8963Init();

    uint8_t who = 0;
    i2cReadBytes(MPU_ADDR, WHO_AM_I_MPU, 1, &who);
    Serial.print("MPU WHO_AM_I: 0x"); Serial.println(who, HEX);

    return true;
}

void readAccelGyro(float& ax, float& ay, float& az, float& gx, float& gy, float& gz, float& tempC) {
    uint8_t buf[14];
    i2cReadBytes(MPU_ADDR, ACCEL_XOUT_H, 14, buf);

    int16_t axraw = (int16_t)((buf[0] << 8) | buf[1]);
    int16_t ayraw = (int16_t)((buf[2] << 8) | buf[3]);
    int16_t azraw = (int16_t)((buf[4] << 8) | buf[5]);
    int16_t traw = (int16_t)((buf[6] << 8) | buf[7]);
    int16_t gxraw = (int16_t)((buf[8] << 8) | buf[9]);
    int16_t gyraw = (int16_t)((buf[10] << 8) | buf[11]);
    int16_t gzraw = (int16_t)((buf[12] << 8) | buf[13]);

    ax = axraw * accelScale;
    ay = ayraw * accelScale;
    az = azraw * accelScale;
    gx = gxraw * gyroScale;
    gy = gyraw * gyroScale;
    gz = gzraw * gyroScale;
    tempC = ((float)traw) / 333.87f + 21.0f;
}

bool readMag(float& mx, float& my, float& mz) {
    uint8_t ext[8] = {0};
    i2cReadBytes(MPU_ADDR, 0x49, 8, ext);

    if (!(ext[0] & 0x01)) return false;

    int16_t mxraw = (int16_t)((ext[2] << 8) | ext[1]);
    int16_t myraw = (int16_t)((ext[4] << 8) | ext[3]);
    int16_t mzraw = (int16_t)((ext[6] << 8) | ext[5]);

    mx = (float)mxraw * magAdj[0] * magScale;
    my = (float)myraw * magAdj[1] * magScale;
    mz = (float)mzraw * magAdj[2] * magScale;

    return true;
}

// ========== RC INPUT HANDLING ==========
void IRAM_ATTR onEdge(RcChannel &ch) {
    int level = digitalRead(ch.pin);
    uint32_t now = micros();

    if (level) {
        ch.riseTime = now;
    } else {
        uint32_t width = now - ch.riseTime;
        if (width >= 800 && width <= 2500) {
            ch.pulseWidth = (uint16_t)width;
            ch.newValue = true;
            ch.lastUpdate = millis();
        }
    }
}

void IRAM_ATTR handleChange0() { onEdge(channels[0]); }
void IRAM_ATTR handleChange1() { onEdge(channels[1]); }
void IRAM_ATTR handleChange2() { onEdge(channels[2]); }
void IRAM_ATTR handleChange3() { onEdge(channels[3]); }
void IRAM_ATTR handleChange4() { onEdge(channels[4]); }
void IRAM_ATTR handleChange5() { onEdge(channels[5]); }

typedef void (*isr_t)();
isr_t isrList[6] = {handleChange0, handleChange1, handleChange2, handleChange3, handleChange4, handleChange5};

void setupChannelPin(uint8_t idx) {
    pinMode(channels[idx].pin, INPUT);
    attachInterrupt(digitalPinToInterrupt(channels[idx].pin), isrList[idx], CHANGE);
}

// ========== ESC/PWM FUNCTIONS ==========
uint32_t usToDuty(uint16_t us) {
    if (us < MIN_US) us = MIN_US;
    if (us > MAX_US) us = MAX_US;
    return (uint32_t)((uint64_t)us * PWM_MAX_DUTY / PERIOD_US);
}

void setupPwmOutputs() {
    bool timerConfigured[2][4] = {};

    for (int i = 0; i < 6; i++) {
        int modeIdx = (escOuts[i].mode == LEDC_LOW_SPEED_MODE) ? 0 : 1;

        if (!timerConfigured[modeIdx][escOuts[i].timer]) {
            ledc_timer_config_t tcfg = {
                .speed_mode = escOuts[i].mode,
                .duty_resolution = (ledc_timer_bit_t)PWM_RES_BITS,
                .timer_num = escOuts[i].timer,
                .freq_hz = PWM_FREQ_HZ,
                .clk_cfg = LEDC_AUTO_CLK
            };
            ledc_timer_config(&tcfg);
            timerConfigured[modeIdx][escOuts[i].timer] = true;
        }

        pinMode(escOuts[i].pin, OUTPUT);
        ledc_channel_config_t ccfg = {
            .gpio_num = escOuts[i].pin,
            .speed_mode = escOuts[i].mode,
            .channel = escOuts[i].channel,
            .intr_type = LEDC_INTR_DISABLE,
            .timer_sel = escOuts[i].timer,
            .duty = usToDuty(MIN_US),
            .hpoint = 0
        };
        ledc_channel_config(&ccfg);
    }
}

void writeEscPulse(int motorIndex, uint16_t us) {
    if (motorIndex < 0 || motorIndex >= 6) return;
    uint32_t duty = usToDuty(us);
    ledc_set_duty(escOuts[motorIndex].mode, escOuts[motorIndex].channel, duty);
    ledc_update_duty(escOuts[motorIndex].mode, escOuts[motorIndex].channel);
}

void writeEscPulseAll(uint16_t us) {
    for (int i = 0; i < 6; i++) {
        writeEscPulse(i, us);
    }
}

// ========== FLIGHT CONTROL FUNCTIONS ==========
// Updated motor mixing with higher minimum throttle and idle speed
void mixMotors(uint16_t throttle, float rollCorrection, float pitchCorrection, float yawCorrection, uint16_t motors[6]) {
    // CRITICAL SAFETY: If throttle below raised deadband, force all motors to minimum
    if (throttle < THROTTLE_DEADBAND) {
        for (int i = 0; i < 6; i++) {
            motors[i] = 1000;
        }
        return;
    }

    // Map throttle range to start at higher minimum
    // Input throttle range: 1200-2000µs
    // Output motor range: 1200-1750µs (75% max for safety)
    uint16_t mappedThrottle = map(throttle, THROTTLE_DEADBAND, 2000, MOTOR_IDLE_SPEED, 1750);
    mappedThrottle = constrain(mappedThrottle, MOTOR_IDLE_SPEED, 1750);

    // Hexacopter motor mixing matrix (+ configuration)
    float mix[6][4] = {
        {1.0, 0.0, 1.0, 1.0}, // Motor 1 (Front)
        {1.0, 0.866, -0.5, -1.0}, // Motor 2 (Front Right)
        {1.0, 0.866, -0.5, 1.0}, // Motor 3 (Back Right)
        {1.0, 0.0, -1.0, -1.0}, // Motor 4 (Back)
        {1.0, -0.866, -0.5, 1.0}, // Motor 5 (Back Left)
        {1.0, -0.866, 0.5, -1.0} // Motor 6 (Front Left)
    };

    for (int i = 0; i < 6; i++) {
        float motorValue = mappedThrottle * mix[i][0] +
                           rollCorrection * mix[i][1] +
                           pitchCorrection * mix[i][2] +
                           yawCorrection * mix[i][3];
        // Ensure motors never go below idle speed when armed
        motors[i] = constrain(motorValue, MOTOR_IDLE_SPEED, 1750);
    }
}

bool checkRcSignal() {
    uint32_t now = millis();
    for (int i = 0; i < 4; i++) {
        if (now - channels[i].lastUpdate > 500) {
            return false;
        }
        if (channels[i].pulseWidth < 800 || channels[i].pulseWidth > 2200) {
            return false;
        }
    }
    return true;
}

// Updated arming conditions for new throttle deadband
bool checkArmingConditions() {
    if (channels[2].pulseWidth > THROTTLE_DEADBAND) return false;  // Use new deadband
    if (!checkRcSignal()) return false;
    if (abs(flight.rollRate) > 10 || abs(flight.pitchRate) > 10) return false;
    return true;
}

void handleArming() {
    static uint32_t armingStart = 0;
    static bool armingSequence = false;

    if (!checkRcSignal()) {
        if (flight.armed) {
            flight.failsafe = true;
            Serial.println("FAILSAFE: RC signal lost!");
        }
        return;
    } else {
        flight.failsafe = false;
    }

    if (!flight.armed) {
        // Updated arming sequence for new throttle deadband
        if (channels[2].pulseWidth < THROTTLE_DEADBAND && channels[3].pulseWidth > 1800) {
            if (!armingSequence) {
                armingSequence = true;
                armingStart = millis();
                Serial.println("Arming sequence started...");
            } else if (millis() - armingStart > 3000) {
                if (checkArmingConditions()) {
                    flight.armed = true;
                    digitalWrite(LED_RED, HIGH);
                    rollPID.reset();
                    pitchPID.reset();
                    yawPID.reset();
                    rollRatePID.reset();
                    pitchRatePID.reset();
                    yawRatePID.reset();
                    altitudePID.reset();
                    flight.targetAltitude = flight.altitude;
                    Serial.println("🚁 ARMED! Motors at idle speed.");
                } else {
                    Serial.println("❌ Arming failed - conditions not met");
                }
                armingSequence = false;
            }
        } else {
            armingSequence = false;
        }
    } else {
        // Updated disarming sequence for new throttle deadband
        if (channels[2].pulseWidth < THROTTLE_DEADBAND && channels[3].pulseWidth < 1200) {
            if (!armingSequence) {
                armingSequence = true;
                armingStart = millis();
                Serial.println("Disarming sequence started...");
            } else if (millis() - armingStart > 2000) {
                flight.armed = false;
                flight.failsafe = false;
                digitalWrite(LED_RED, LOW);
                Serial.println("🛑 DISARMED!");
                armingSequence = false;
            }
        } else {
            armingSequence = false;
        }
    }
}

void updateFlightMode() {
    uint16_t modeSwitch = channels[4].pulseWidth;
    if (modeSwitch < 1300) {
        flight.mode = STABILIZE;
        digitalWrite(LED_GREEN, LOW);
    } else if (modeSwitch < 1700) {
        flight.mode = ALTITUDE_HOLD;
        digitalWrite(LED_GREEN, HIGH);
    } else {
        flight.mode = ACRO;
        digitalWrite(LED_GREEN, !digitalRead(LED_GREEN));
    }
}

void calculateAttitude() {
    static float alpha = 0.98;
    static uint32_t lastTime = micros();
    uint32_t now = micros();
    float dt = (now - lastTime) / 1000000.0;
    if (dt > 0.1) dt = 0.01;

    flight.rollRate = gx - gxOffset;
    flight.pitchRate = gy - gyOffset;
    flight.yawRate = gz - gzOffset;

    flight.roll += flight.rollRate * dt;
    flight.pitch += flight.pitchRate * dt;
    flight.yaw += flight.yawRate * dt;

    if (flight.yaw > 180) flight.yaw -= 360;
    if (flight.yaw < -180) flight.yaw += 360;

    float accRoll = atan2(ay - ayOffset, az - azOffset) * 180.0 / PI;
    float accPitch = atan2(-(ax - axOffset), sqrt(pow(ay - ayOffset, 2) + pow(az - azOffset, 2))) * 180.0 / PI;

    flight.roll = alpha * flight.roll + (1.0 - alpha) * accRoll;
    flight.pitch = alpha * flight.pitch + (1.0 - alpha) * accPitch;

    lastTime = now;
}

// Enhanced flight control loop with higher throttle deadband
void flightControlLoop() {
    readAccelGyro(ax, ay, az, gx, gy, gz, tempC);
    readMag(mx, my, mz);
    flight.altitude = bmp.readAltitude(1013.25);

    calculateAttitude();
    handleArming();
    updateFlightMode();

    float desiredRoll = map(channels[0].pulseWidth, channels[0].minVal, channels[0].maxVal, -30, 30);
    float desiredPitch = map(channels[1].pulseWidth, channels[1].minVal, channels[1].maxVal, -30, 30);
    float desiredYawRate = map(channels[3].pulseWidth, channels[3].minVal, channels[3].maxVal, -100, 100);
    uint16_t throttleInput = constrain(channels[2].pulseWidth, 1000, 2000);

    desiredRoll = constrain(desiredRoll, -45, 45);
    desiredPitch = constrain(desiredPitch, -45, 45);
    desiredYawRate = constrain(desiredYawRate, -180, 180);

    // Critical safety check with new deadband - motors maintain idle speed when armed
    if (!flight.armed || flight.failsafe || throttleInput < THROTTLE_DEADBAND) {
        writeEscPulseAll(1000);  // Stop all motors when disarmed or in failsafe
        if (!flight.armed) {
            rollPID.reset();
            pitchPID.reset();
            yawPID.reset();
            rollRatePID.reset();
            pitchRatePID.reset();
            yawRatePID.reset();
            altitudePID.reset();
        }
        flight.lastUpdate = millis();
        return;
    }

    float rollCorrection = 0, pitchCorrection = 0, yawCorrection = 0;
    float altitudeCorrection = 0;

    switch (flight.mode) {
        case STABILIZE:
            rollCorrection = rollPID.calculate(desiredRoll, flight.roll, throttleInput);
            pitchCorrection = pitchPID.calculate(desiredPitch, flight.pitch, throttleInput);
            yawCorrection = yawRatePID.calculate(desiredYawRate, flight.yawRate);
            break;

        case ALTITUDE_HOLD:
            if (throttleInput > 1600) {
                flight.targetAltitude += 0.5;
            } else if (throttleInput < 1400) {
                flight.targetAltitude -= 0.5;
            }

            altitudeCorrection = altitudePID.calculate(flight.targetAltitude, flight.altitude