iforce2d Chris
/
04_I2C_MPU6050
YMFC-AL implementation in mbed.
main.cpp
- Committer:
- iforce2d
- Date:
- 2019-10-04
- Revision:
- 1:411d267f9d32
- Parent:
- 0:f76c26307f9a
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]); }*/ }