File content as of revision 1:411d267f9d32:

#include "mbed.h"
#include "PPM.h"

I2C i2c(PB_11, PB_10);  // sda, scl
Serial pc(PA_9, PA_10, 115200); // tx, rx
DigitalOut led_green(PB_3);
DigitalOut led_red(PB_4);
PPM ppm(PA_0);

DigitalOut motorPin1(PA_8);
DigitalOut motorPin2(PA_11);
DigitalOut motorPin3(PB_6);
DigitalOut motorPin4(PB_7);
DigitalOut motorPin5(PB_8);
DigitalOut motorPin6(PB_9);

DigitalOut buzzer(PA_12);

#define ROLL  0
#define PITCH 1
#define YAW   2

#define RC_ROLL      (ppm.channels[0])
#define RC_PITCH     (ppm.channels[1])
#define RC_THROTTLE  (ppm.channels[2])
#define RC_YAW       (ppm.channels[3])
#define RC_DEADBAND_US 8

#define GYRO_CALIB_SAMPLES 4000
#define MPU6050 (0x68 << 1)

float pid_p_gain_roll = 1.3;               //Gain setting for the roll P-controller
float pid_i_gain_roll = 0.04;              //Gain setting for the roll I-controller
float pid_d_gain_roll = 18.0;              //Gain setting for the roll D-controller
int pid_max_roll = 400;                    //Maximum output of the PID-controller (+/-)

float pid_p_gain_pitch = pid_p_gain_roll;  //Gain setting for the pitch P-controller.
float pid_i_gain_pitch = pid_i_gain_roll;  //Gain setting for the pitch I-controller.
float pid_d_gain_pitch = pid_d_gain_roll;  //Gain setting for the pitch D-controller.
int pid_max_pitch = pid_max_roll;          //Maximum output of the PID-controller (+/-)

float pid_p_gain_yaw = 4.0;                //Gain setting for the pitch P-controller. //4.0
float pid_i_gain_yaw = 0.02;               //Gain setting for the pitch I-controller. //0.02
float pid_d_gain_yaw = 0.0;                //Gain setting for the pitch D-controller.
int pid_max_yaw = 400;                     //Maximum output of the PID-controller (+/-)

int cal_int;
double gyro_cal[3];
int16_t acc_raw[3], gyro_raw[3];
float acc_smoothed[3], gyro_smoothed[3];

Timer armingTimer;

Timer timer;

float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll;
float acc_total_vector;

float roll_level_adjust, pitch_level_adjust;
float pid_i_mem_roll, pid_roll_setpoint, pid_output_roll, pid_last_roll_d_error;
float pid_i_mem_pitch, pid_pitch_setpoint, pid_output_pitch, pid_last_pitch_d_error;
float pid_i_mem_yaw, pid_yaw_setpoint, pid_output_yaw, pid_last_yaw_d_error;

int constrain( int val, int lower, int upper )
{
    if ( val < lower ) return lower;
    if ( val > upper ) return upper;
    return val;
}

void beepStart() 
{
    buzzer = 0;
}

void beepStop() 
{
    buzzer = 1;
}

void beep(int ms) 
{
    beepStart();
    wait_ms(ms);
    beepStop();
}

void setIMURegisters() 
{    
    char cmd[2];
    
    cmd[0] = 0x6B;                                                          //We want to write to the PWR_MGMT_1 register (6B hex)
    cmd[1] = 0x00;                                                          //Set the register bits as 00000000 to activate the gyro
    i2c.write(MPU6050, cmd, 2);
    
    cmd[0] = 0x1B;                                                          //We want to write to the GYRO_CONFIG register (1B hex)
    cmd[1] = 0x08;                                                          //Set the register bits as 00001000 (500dps full scale)
    i2c.write(MPU6050, cmd, 2);
    
    cmd[0] = 0x1C;                                                          //We want to write to the ACCEL_CONFIG register (1A hex)
    cmd[1] = 0x10;                                                          //Set the register bits as 00010000 (+/- 8g full scale range)
    i2c.write(MPU6050, cmd, 2);
    
    //Let's perform a random register check to see if the values are written correct
    cmd[0] = 0x1B;                                                          //Start reading at register 0x1B
    i2c.write(MPU6050, cmd, 1);    
    i2c.read(MPU6050, cmd, 1);
    if (cmd[0] != 0x08) {                                                   //Check if the value is 0x08
        led_red = 0;                                                        //Turn on the warning led
        pc.printf("Gyro init failed! (%d)\n", cmd[0]);
        while (true)
            wait(1);                                                      //Stay in this loop for ever
    }
    
    cmd[0] = 0x1A;                                                          //We want to write to the CONFIG register (1A hex)
    cmd[1] = 0x03;                                                          //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
    i2c.write(MPU6050, cmd, 2);
}

void readIMU(bool calibrated = true)
{    
    char cmd[14];

    cmd[0] = 0x3B;                                                          //Start reading at register 43h and auto increment with every read.
    i2c.write(MPU6050, cmd, 1);
    i2c.read(MPU6050, cmd, 14);                                             //Read 14 bytes from the gyro.
    
    acc_raw[0] = (cmd[0] << 8) | cmd[1];                               //Add the low and high byte to the acc_x variable.
    acc_raw[1] = (cmd[2] << 8) | cmd[3];                               //Add the low and high byte to the acc_y variable.
    acc_raw[2] = (cmd[4] << 8) | cmd[5];                               //Add the low and high byte to the acc_z variable.
    
    gyro_raw[0] = (cmd[8] << 8) |  cmd[9];                              //Read high and low part of the angular data.
    gyro_raw[1] = (cmd[10] << 8) | cmd[11];                              //Read high and low part of the angular data.
    gyro_raw[2] = (cmd[12] << 8) | cmd[13];                              //Read high and low part of the angular data.
    
    if ( calibrated ) {
        for (int i = 0; i < 3; i++)
            gyro_raw[i] -= gyro_cal[i];                                       
    }
}

void calibrateGyro() 
{
    for (int i = 0; i < 3; i++) 
        gyro_cal[i] = 0;
    
    for (int s = 0; s < GYRO_CALIB_SAMPLES; s++) {                            //Take GYRO_CALIB_SAMPLES readings for calibration.
        if (s % 50 == 0)
            led_green = 1 - led_green;                                          //Change the led status to indicate calibration.
        readIMU(false);                                                        //Read the gyro output.
        for (int i = 0; i < 3; i++)
            gyro_cal[i] += gyro_raw[i];                                       //Ad roll value to gyro_roll_cal.
        wait_us(3);                                                               //Wait 3 milliseconds before the next loop.
    }
    
    for (int i = 0; i < 3; i++)
        gyro_cal[i] /= (float)GYRO_CALIB_SAMPLES;                                                 //Divide the roll total by GYRO_CALIB_SAMPLES.
}

bool checkArmStateChange(bool armed) 
{
    static int armingState = 0;
    
    if ( RC_THROTTLE > 1100 )
        return false;
    
    if ( ! armed && RC_YAW > 1900 ) {
        if ( armingState == 0 ) {
            armingState = 1;
            armingTimer.reset();
            armingTimer.start();
            pc.printf("Started arming\n");
        }
        else {
            uint16_t time = armingTimer.read_ms();
            if ( time > 2000 ) {
                armed = true;
                armingState = 0;
                pc.printf("ARMED\n");
                //beep(300);
                return true;
            }
        }
    }
    else if ( armed && RC_YAW < 1100 ) {
        if ( armingState == 0 ) {
            armingState = -1;
            armingTimer.reset();
            armingTimer.start();
            pc.printf("Started disarming\n");
        }
        else {
            uint16_t time = armingTimer.read_ms();
            if ( time > 2000 ) {
                armed = false;
                armingState = 0;
                pc.printf("DISARMED\n");
                //beep(50);
                //wait_ms(50);
                //beep(50);
                return false;
            }
        }
    }
    else {
        armingState = 0;
        armingTimer.reset();
    }
    
    return armed;
}

bool getAutoLevelState()
{
    return ppm.channels[5] > 1600;
}

int afterDeadband(int in)
{
    int lower = 1500 - RC_DEADBAND_US;
    int upper = 1500 + RC_DEADBAND_US;
    if ( in < lower )
        return in - lower;
    if ( in > upper )
        return in - upper;
    return 0;
}

void resetPID()
{
    pid_i_mem_roll = 0;
    pid_last_roll_d_error = 0;
    pid_i_mem_pitch = 0;
    pid_last_pitch_d_error = 0;
    pid_i_mem_yaw = 0;
    pid_last_yaw_d_error = 0;
}

void calculatePID()
{
    //Roll calculations
    float pid_error_temp = gyro_smoothed[ROLL] - pid_roll_setpoint;
    pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
    if(pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
    else if(pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;
    
    pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
    if(pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
    else if(pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;
    
    pid_last_roll_d_error = pid_error_temp;
    
    //Pitch calculations
    pid_error_temp = gyro_smoothed[PITCH] - pid_pitch_setpoint;
    pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
    if(pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
    else if(pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;
    
    pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
    if(pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
    else if(pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;
    
    pid_last_pitch_d_error = pid_error_temp;
    
    //Yaw calculations
    pid_error_temp = gyro_smoothed[YAW] - pid_yaw_setpoint;
    pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
    if(pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
    else if(pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;
    
    pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pid_error_temp - pid_last_yaw_d_error);
    if(pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
    else if(pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;
    
    pid_last_yaw_d_error = pid_error_temp;
}

int main() {
    
    i2c.frequency(400000);
    
    buzzer = 1;
    led_red = 1;
    led_green = 1;
    
    motorPin1 = 0;
    motorPin2 = 0;
    motorPin3 = 0;
    motorPin4 = 0;
    motorPin5 = 0;
    motorPin6 = 0;
    
    armingTimer.reset();
    
    beep(50);
        
    pc.printf("setIMURegisters\n");    
    setIMURegisters();
    
    pc.printf("calibrateGyro\n");
    calibrateGyro();
    
    pc.printf("Finished gyro calibration (%f, %f, %f)\n", gyro_cal[0], gyro_cal[1], gyro_cal[2]);
    
    timer.reset();
    timer.start();
    
    for (int i = 0; i < 3; i++)
        gyro_smoothed[i] = 0;
    angle_pitch = 0;
    angle_roll = 0;
        
    beep(50);
    wait_ms(50);
    beep(50);
    
    bool armed = false;
    bool armedLastTime = armed;
    
    bool autoLevel = getAutoLevelState();
    bool autoLevelLastTime = autoLevel;
    
    unsigned long oneShotCounter = 0;
    
    unsigned long loopCounter = 0;
    
    float initialAccUptake = 0.8;
    
    while (true) {
        
        //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
        //pc.printf("Accel (%f, %f)\n", angle_pitch, angle_roll);
        
        armed = checkArmStateChange(armed);
        led_green = ! armed;
        
        if ( ! armedLastTime && armed )
            resetPID();
        if ( armed != armedLastTime ) {
            oneShotCounter = 20;
            beepStart();
        }
        armedLastTime = armed;
        
        autoLevel = getAutoLevelState();
        if ( autoLevel != autoLevelLastTime ) {
            oneShotCounter = 4;
            beepStart();
        }
        autoLevelLastTime = autoLevel;
        
        if ( oneShotCounter > 0 ) {
            if ( --oneShotCounter == 0 ) {
                beepStop();
            }
        }
        
        const float uptake = 0.3;
        for (int i = 0; i < 3; i++)
            gyro_smoothed[i] = (gyro_smoothed[i] * (1-uptake)) + ((gyro_raw[i] / 65.5) * uptake);   //Gyro pid input is deg/sec.
            
            
        //Gyro angle calculations
        //0.0000611 = 1 / (250Hz / 65.5)
        angle_pitch += gyro_raw[ROLL] * 0.0000611;                                    //Calculate the traveled pitch angle and add this to the angle_pitch variable.
        angle_roll += gyro_raw[PITCH] * 0.0000611;                                      //Calculate the traveled roll angle and add this to the angle_roll variable.
        
        //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
        float new_angle_pitch = angle_pitch + angle_roll * sin(gyro_raw[YAW] * 0.000001066);                  //If the IMU has yawed transfer the roll angle to the pitch angel.
        angle_roll -= angle_pitch * sin(gyro_raw[YAW] * 0.000001066);                  //If the IMU has yawed transfer the pitch angle to the roll angel.
        angle_pitch = new_angle_pitch;
        
        //Accelerometer angle calculations
        acc_total_vector = sqrtf((acc_raw[0]*acc_raw[0])+(acc_raw[1]*acc_raw[1])+(acc_raw[2]*acc_raw[2]));       //Calculate the total accelerometer vector.
        
        if (abs(acc_raw[PITCH]) < acc_total_vector) {                                        //Prevent the asin function to produce a NaN
            angle_pitch_acc = asin((float)acc_raw[PITCH]/acc_total_vector)* 57.296;          //Calculate the pitch angle.
        }
        if (abs(acc_raw[ROLL]) < acc_total_vector) {                                        //Prevent the asin function to produce a NaN
            angle_roll_acc = asin((float)acc_raw[ROLL]/acc_total_vector)* -57.296;          //Calculate the roll angle.
        }
        
        //Place the MPU-6050 spirit level and note the values in the following two lines for calibration.
        angle_pitch_acc -= 0.0;                                                   //Accelerometer calibration value for pitch.
        angle_roll_acc -= 0.0;                                                    //Accelerometer calibration value for roll.
        
        initialAccUptake *= 0.98;
        
        float acc_uptake = 0.0004 + initialAccUptake;
        angle_pitch = angle_pitch * (1-acc_uptake) + angle_pitch_acc * acc_uptake;            //Correct the drift of the gyro pitch angle with the accelerometer pitch angle.
        angle_roll = angle_roll * (1-acc_uptake) + angle_roll_acc * acc_uptake;               //Correct the drift of the gyro roll angle with the accelerometer roll angle.
        
        pitch_level_adjust = angle_pitch * 15;                                    //Calculate the pitch angle correction
        roll_level_adjust = angle_roll * 15;                                      //Calculate the roll angle correction
        
        if ( ! autoLevel ){                                                          //If the quadcopter is not in auto-level mode
            pitch_level_adjust = 0;                                                 //Set the pitch angle correction to zero.
            roll_level_adjust = 0;                                                  //Set the roll angle correcion to zero.
        }
            
           
           
        //The PID set point in degrees per second is determined by the roll receiver input.
        //In the case of deviding by 3 the max roll rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
        pid_roll_setpoint = afterDeadband(RC_ROLL);        
        pid_roll_setpoint -= roll_level_adjust;                                   //Subtract the angle correction from the standardized receiver roll input value.
        pid_roll_setpoint /= 3.0;                                                 //Divide the setpoint for the PID roll controller by 3 to get angles in degrees.
        
        //The PID set point in degrees per second is determined by the pitch receiver input.
        //In the case of deviding by 3 the max pitch rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
        pid_pitch_setpoint = afterDeadband(RC_PITCH);        
        pid_pitch_setpoint -= pitch_level_adjust;                                  //Subtract the angle correction from the standardized receiver pitch input value.
        pid_pitch_setpoint /= 3.0;                                                 //Divide the setpoint for the PID pitch controller by 3 to get angles in degrees.
        
        //The PID set point in degrees per second is determined by the yaw receiver input.
        //In the case of dividing by 3 the max yaw rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
        pid_yaw_setpoint = 0;
        //We need a little dead band of 16us for better results.
        if (RC_THROTTLE > 1050) { //Do not yaw when turning off the motors.
            pid_yaw_setpoint = afterDeadband(RC_YAW) / 3.0;
        }
        
        calculatePID();           
           
        int pwm[6];
        
        if (armed) {                                                          //The motors are started.
            int throttle = RC_THROTTLE;
            if (throttle > 1800) 
                throttle = 1800;                                   //We need some room to keep full control at full throttle.
            pwm[0] = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
            pwm[1] = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
            pwm[2] = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
            pwm[3] = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)
            pwm[4] = 1000;
            pwm[5] = 1000;
            
            for (int i = 0; i < 6; i++)
                pwm[i] = constrain( pwm[i], 1100, 2000);                                         //Keep the motors running.
        }        
        else {
            for (int i = 0; i < 6; i++)
                pwm[i] = 1000;                                                           //If start is not 2 keep a 1000us pulse for ess-1.
        }
    
    
    
        
        while ( timer.read_us() < 4000 )
            ; // wait
            
        motorPin1 = motorPin2 = motorPin3 = motorPin4 = 1;
        timer.reset();
        
        readIMU();
        
        if ( loopCounter++ % 20 == 0 ) {
            //pc.printf("%d %d %d %d\n", pwm[0], pwm[1], pwm[2], pwm[3]);
            pc.printf("%f %f\n", angle_roll, angle_pitch );
            //pc.printf("%d\n", pwm[0] );
            //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
            //pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]);
        }
        
        bool doneOutput = false;
        while ( ! doneOutput ) {
            int now = timer.read_us();
            int c = 0;
            if ( now >= pwm[0] ) { motorPin1 = 0; c++; }
            if ( now >= pwm[1] ) { motorPin2 = 0; c++; }
            if ( now >= pwm[2] ) { motorPin3 = 0; c++; }
            if ( now >= pwm[3] ) { motorPin4 = 0; c++; }
            if ( now >= pwm[4] ) { motorPin5 = 0; c++; }
            if ( now >= pwm[5] ) { motorPin6 = 0; c++; }
            doneOutput = c >= 6;
        }
        
    }
    
    /*while (true) {
        pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]);
    }*/
}