iforce2d Chris / Mbed 2 deprecated 04_I2C_MPU6050

YMFC-AL implementation in mbed.

Dependencies:   mbed

main.cpp@1:411d267f9d32, 2019-10-04 (annotated)

Committer:
iforce2d
Date:
Fri Oct 04 17:04:41 2019 +0000
Revision:
1:411d267f9d32
Parent:
0:f76c26307f9a 
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:f76c26307f9a 1 #include "mbed.h"
iforce2d 1:411d267f9d32 2 #include "PPM.h"
iforce2d 1:411d267f9d32 3
iforce2d 1:411d267f9d32 4 I2C i2c(PB_11, PB_10); // sda, scl
iforce2d 1:411d267f9d32 5 Serial pc(PA_9, PA_10, 115200); // tx, rx
iforce2d 1:411d267f9d32 6 DigitalOut led_green(PB_3);
iforce2d 1:411d267f9d32 7 DigitalOut led_red(PB_4);
iforce2d 1:411d267f9d32 8 PPM ppm(PA_0);
iforce2d 1:411d267f9d32 9
iforce2d 1:411d267f9d32 10 DigitalOut motorPin1(PA_8);
iforce2d 1:411d267f9d32 11 DigitalOut motorPin2(PA_11);
iforce2d 1:411d267f9d32 12 DigitalOut motorPin3(PB_6);
iforce2d 1:411d267f9d32 13 DigitalOut motorPin4(PB_7);
iforce2d 1:411d267f9d32 14 DigitalOut motorPin5(PB_8);
iforce2d 1:411d267f9d32 15 DigitalOut motorPin6(PB_9);
iforce2d 1:411d267f9d32 16
iforce2d 1:411d267f9d32 17 DigitalOut buzzer(PA_12);
iforce2d 1:411d267f9d32 18
iforce2d 1:411d267f9d32 19 #define ROLL 0
iforce2d 1:411d267f9d32 20 #define PITCH 1
iforce2d 1:411d267f9d32 21 #define YAW 2
iforce2d 1:411d267f9d32 22
iforce2d 1:411d267f9d32 23 #define RC_ROLL (ppm.channels[0])
iforce2d 1:411d267f9d32 24 #define RC_PITCH (ppm.channels[1])
iforce2d 1:411d267f9d32 25 #define RC_THROTTLE (ppm.channels[2])
iforce2d 1:411d267f9d32 26 #define RC_YAW (ppm.channels[3])
iforce2d 1:411d267f9d32 27 #define RC_DEADBAND_US 8
iforce2d 1:411d267f9d32 28
iforce2d 1:411d267f9d32 29 #define GYRO_CALIB_SAMPLES 4000
iforce2d 1:411d267f9d32 30 #define MPU6050 (0x68 << 1)
iforce2d 1:411d267f9d32 31
iforce2d 1:411d267f9d32 32 float pid_p_gain_roll = 1.3; //Gain setting for the roll P-controller
iforce2d 1:411d267f9d32 33 float pid_i_gain_roll = 0.04; //Gain setting for the roll I-controller
iforce2d 1:411d267f9d32 34 float pid_d_gain_roll = 18.0; //Gain setting for the roll D-controller
iforce2d 1:411d267f9d32 35 int pid_max_roll = 400; //Maximum output of the PID-controller (+/-)
iforce2d 1:411d267f9d32 36
iforce2d 1:411d267f9d32 37 float pid_p_gain_pitch = pid_p_gain_roll; //Gain setting for the pitch P-controller.
iforce2d 1:411d267f9d32 38 float pid_i_gain_pitch = pid_i_gain_roll; //Gain setting for the pitch I-controller.
iforce2d 1:411d267f9d32 39 float pid_d_gain_pitch = pid_d_gain_roll; //Gain setting for the pitch D-controller.
iforce2d 1:411d267f9d32 40 int pid_max_pitch = pid_max_roll; //Maximum output of the PID-controller (+/-)
iforce2d 1:411d267f9d32 41
iforce2d 1:411d267f9d32 42 float pid_p_gain_yaw = 4.0; //Gain setting for the pitch P-controller. //4.0
iforce2d 1:411d267f9d32 43 float pid_i_gain_yaw = 0.02; //Gain setting for the pitch I-controller. //0.02
iforce2d 1:411d267f9d32 44 float pid_d_gain_yaw = 0.0; //Gain setting for the pitch D-controller.
iforce2d 1:411d267f9d32 45 int pid_max_yaw = 400; //Maximum output of the PID-controller (+/-)
iforce2d 1:411d267f9d32 46
iforce2d 1:411d267f9d32 47 int cal_int;
iforce2d 1:411d267f9d32 48 double gyro_cal[3];
iforce2d 1:411d267f9d32 49 int16_t acc_raw[3], gyro_raw[3];
iforce2d 1:411d267f9d32 50 float acc_smoothed[3], gyro_smoothed[3];
iforce2d 1:411d267f9d32 51
iforce2d 1:411d267f9d32 52 Timer armingTimer;
iforce2d 1:411d267f9d32 53
iforce2d 1:411d267f9d32 54 Timer timer;
iforce2d 1:411d267f9d32 55
iforce2d 1:411d267f9d32 56 float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll;
iforce2d 1:411d267f9d32 57 float acc_total_vector;
iforce2d 1:411d267f9d32 58
iforce2d 1:411d267f9d32 59 float roll_level_adjust, pitch_level_adjust;
iforce2d 1:411d267f9d32 60 float pid_i_mem_roll, pid_roll_setpoint, pid_output_roll, pid_last_roll_d_error;
iforce2d 1:411d267f9d32 61 float pid_i_mem_pitch, pid_pitch_setpoint, pid_output_pitch, pid_last_pitch_d_error;
iforce2d 1:411d267f9d32 62 float pid_i_mem_yaw, pid_yaw_setpoint, pid_output_yaw, pid_last_yaw_d_error;
iforce2d 1:411d267f9d32 63
iforce2d 1:411d267f9d32 64 int constrain( int val, int lower, int upper )
iforce2d 1:411d267f9d32 65 {
iforce2d 1:411d267f9d32 66 if ( val < lower ) return lower;
iforce2d 1:411d267f9d32 67 if ( val > upper ) return upper;
iforce2d 1:411d267f9d32 68 return val;
iforce2d 1:411d267f9d32 69 }
iforce2d 1:411d267f9d32 70
iforce2d 1:411d267f9d32 71 void beepStart()
iforce2d 1:411d267f9d32 72 {
iforce2d 1:411d267f9d32 73 buzzer = 0;
iforce2d 1:411d267f9d32 74 }
iforce2d 1:411d267f9d32 75
iforce2d 1:411d267f9d32 76 void beepStop()
iforce2d 1:411d267f9d32 77 {
iforce2d 1:411d267f9d32 78 buzzer = 1;
iforce2d 1:411d267f9d32 79 }
iforce2d 1:411d267f9d32 80
iforce2d 1:411d267f9d32 81 void beep(int ms)
iforce2d 1:411d267f9d32 82 {
iforce2d 1:411d267f9d32 83 beepStart();
iforce2d 1:411d267f9d32 84 wait_ms(ms);
iforce2d 1:411d267f9d32 85 beepStop();
iforce2d 1:411d267f9d32 86 }
iforce2d 1:411d267f9d32 87
iforce2d 1:411d267f9d32 88 void setIMURegisters()
iforce2d 1:411d267f9d32 89 {
iforce2d 1:411d267f9d32 90 char cmd[2];
iforce2d 1:411d267f9d32 91
iforce2d 1:411d267f9d32 92 cmd[0] = 0x6B; //We want to write to the PWR_MGMT_1 register (6B hex)
iforce2d 1:411d267f9d32 93 cmd[1] = 0x00; //Set the register bits as 00000000 to activate the gyro
iforce2d 1:411d267f9d32 94 i2c.write(MPU6050, cmd, 2);
iforce2d 1:411d267f9d32 95
iforce2d 1:411d267f9d32 96 cmd[0] = 0x1B; //We want to write to the GYRO_CONFIG register (1B hex)
iforce2d 1:411d267f9d32 97 cmd[1] = 0x08; //Set the register bits as 00001000 (500dps full scale)
iforce2d 1:411d267f9d32 98 i2c.write(MPU6050, cmd, 2);
iforce2d 1:411d267f9d32 99
iforce2d 1:411d267f9d32 100 cmd[0] = 0x1C; //We want to write to the ACCEL_CONFIG register (1A hex)
iforce2d 1:411d267f9d32 101 cmd[1] = 0x10; //Set the register bits as 00010000 (+/- 8g full scale range)
iforce2d 1:411d267f9d32 102 i2c.write(MPU6050, cmd, 2);
iforce2d 1:411d267f9d32 103
iforce2d 1:411d267f9d32 104 //Let's perform a random register check to see if the values are written correct
iforce2d 1:411d267f9d32 105 cmd[0] = 0x1B; //Start reading at register 0x1B
iforce2d 1:411d267f9d32 106 i2c.write(MPU6050, cmd, 1);
iforce2d 1:411d267f9d32 107 i2c.read(MPU6050, cmd, 1);
iforce2d 1:411d267f9d32 108 if (cmd[0] != 0x08) { //Check if the value is 0x08
iforce2d 1:411d267f9d32 109 led_red = 0; //Turn on the warning led
iforce2d 1:411d267f9d32 110 pc.printf("Gyro init failed! (%d)\n", cmd[0]);
iforce2d 1:411d267f9d32 111 while (true)
iforce2d 1:411d267f9d32 112 wait(1); //Stay in this loop for ever
iforce2d 1:411d267f9d32 113 }
iforce2d 1:411d267f9d32 114
iforce2d 1:411d267f9d32 115 cmd[0] = 0x1A; //We want to write to the CONFIG register (1A hex)
iforce2d 1:411d267f9d32 116 cmd[1] = 0x03; //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
iforce2d 1:411d267f9d32 117 i2c.write(MPU6050, cmd, 2);
iforce2d 1:411d267f9d32 118 }
iforce2d 1:411d267f9d32 119
iforce2d 1:411d267f9d32 120 void readIMU(bool calibrated = true)
iforce2d 1:411d267f9d32 121 {
iforce2d 1:411d267f9d32 122 char cmd[14];
mbed_official 0:f76c26307f9a 123
iforce2d 1:411d267f9d32 124 cmd[0] = 0x3B; //Start reading at register 43h and auto increment with every read.
iforce2d 1:411d267f9d32 125 i2c.write(MPU6050, cmd, 1);
iforce2d 1:411d267f9d32 126 i2c.read(MPU6050, cmd, 14); //Read 14 bytes from the gyro.
iforce2d 1:411d267f9d32 127
iforce2d 1:411d267f9d32 128 acc_raw[0] = (cmd[0] << 8) | cmd[1]; //Add the low and high byte to the acc_x variable.
iforce2d 1:411d267f9d32 129 acc_raw[1] = (cmd[2] << 8) | cmd[3]; //Add the low and high byte to the acc_y variable.
iforce2d 1:411d267f9d32 130 acc_raw[2] = (cmd[4] << 8) | cmd[5]; //Add the low and high byte to the acc_z variable.
iforce2d 1:411d267f9d32 131
iforce2d 1:411d267f9d32 132 gyro_raw[0] = (cmd[8] << 8) | cmd[9]; //Read high and low part of the angular data.
iforce2d 1:411d267f9d32 133 gyro_raw[1] = (cmd[10] << 8) | cmd[11]; //Read high and low part of the angular data.
iforce2d 1:411d267f9d32 134 gyro_raw[2] = (cmd[12] << 8) | cmd[13]; //Read high and low part of the angular data.
iforce2d 1:411d267f9d32 135
iforce2d 1:411d267f9d32 136 if ( calibrated ) {
iforce2d 1:411d267f9d32 137 for (int i = 0; i < 3; i++)
iforce2d 1:411d267f9d32 138 gyro_raw[i] -= gyro_cal[i];
iforce2d 1:411d267f9d32 139 }
iforce2d 1:411d267f9d32 140 }
iforce2d 1:411d267f9d32 141
iforce2d 1:411d267f9d32 142 void calibrateGyro()
iforce2d 1:411d267f9d32 143 {
iforce2d 1:411d267f9d32 144 for (int i = 0; i < 3; i++)
iforce2d 1:411d267f9d32 145 gyro_cal[i] = 0;
iforce2d 1:411d267f9d32 146
iforce2d 1:411d267f9d32 147 for (int s = 0; s < GYRO_CALIB_SAMPLES; s++) { //Take GYRO_CALIB_SAMPLES readings for calibration.
iforce2d 1:411d267f9d32 148 if (s % 50 == 0)
iforce2d 1:411d267f9d32 149 led_green = 1 - led_green; //Change the led status to indicate calibration.
iforce2d 1:411d267f9d32 150 readIMU(false); //Read the gyro output.
iforce2d 1:411d267f9d32 151 for (int i = 0; i < 3; i++)
iforce2d 1:411d267f9d32 152 gyro_cal[i] += gyro_raw[i]; //Ad roll value to gyro_roll_cal.
iforce2d 1:411d267f9d32 153 wait_us(3); //Wait 3 milliseconds before the next loop.
iforce2d 1:411d267f9d32 154 }
iforce2d 1:411d267f9d32 155
iforce2d 1:411d267f9d32 156 for (int i = 0; i < 3; i++)
iforce2d 1:411d267f9d32 157 gyro_cal[i] /= (float)GYRO_CALIB_SAMPLES; //Divide the roll total by GYRO_CALIB_SAMPLES.
iforce2d 1:411d267f9d32 158 }
mbed_official 0:f76c26307f9a 159
iforce2d 1:411d267f9d32 160 bool checkArmStateChange(bool armed)
iforce2d 1:411d267f9d32 161 {
iforce2d 1:411d267f9d32 162 static int armingState = 0;
iforce2d 1:411d267f9d32 163
iforce2d 1:411d267f9d32 164 if ( RC_THROTTLE > 1100 )
iforce2d 1:411d267f9d32 165 return false;
iforce2d 1:411d267f9d32 166
iforce2d 1:411d267f9d32 167 if ( ! armed && RC_YAW > 1900 ) {
iforce2d 1:411d267f9d32 168 if ( armingState == 0 ) {
iforce2d 1:411d267f9d32 169 armingState = 1;
iforce2d 1:411d267f9d32 170 armingTimer.reset();
iforce2d 1:411d267f9d32 171 armingTimer.start();
iforce2d 1:411d267f9d32 172 pc.printf("Started arming\n");
iforce2d 1:411d267f9d32 173 }
iforce2d 1:411d267f9d32 174 else {
iforce2d 1:411d267f9d32 175 uint16_t time = armingTimer.read_ms();
iforce2d 1:411d267f9d32 176 if ( time > 2000 ) {
iforce2d 1:411d267f9d32 177 armed = true;
iforce2d 1:411d267f9d32 178 armingState = 0;
iforce2d 1:411d267f9d32 179 pc.printf("ARMED\n");
iforce2d 1:411d267f9d32 180 //beep(300);
iforce2d 1:411d267f9d32 181 return true;
iforce2d 1:411d267f9d32 182 }
iforce2d 1:411d267f9d32 183 }
iforce2d 1:411d267f9d32 184 }
iforce2d 1:411d267f9d32 185 else if ( armed && RC_YAW < 1100 ) {
iforce2d 1:411d267f9d32 186 if ( armingState == 0 ) {
iforce2d 1:411d267f9d32 187 armingState = -1;
iforce2d 1:411d267f9d32 188 armingTimer.reset();
iforce2d 1:411d267f9d32 189 armingTimer.start();
iforce2d 1:411d267f9d32 190 pc.printf("Started disarming\n");
iforce2d 1:411d267f9d32 191 }
iforce2d 1:411d267f9d32 192 else {
iforce2d 1:411d267f9d32 193 uint16_t time = armingTimer.read_ms();
iforce2d 1:411d267f9d32 194 if ( time > 2000 ) {
iforce2d 1:411d267f9d32 195 armed = false;
iforce2d 1:411d267f9d32 196 armingState = 0;
iforce2d 1:411d267f9d32 197 pc.printf("DISARMED\n");
iforce2d 1:411d267f9d32 198 //beep(50);
iforce2d 1:411d267f9d32 199 //wait_ms(50);
iforce2d 1:411d267f9d32 200 //beep(50);
iforce2d 1:411d267f9d32 201 return false;
iforce2d 1:411d267f9d32 202 }
iforce2d 1:411d267f9d32 203 }
iforce2d 1:411d267f9d32 204 }
iforce2d 1:411d267f9d32 205 else {
iforce2d 1:411d267f9d32 206 armingState = 0;
iforce2d 1:411d267f9d32 207 armingTimer.reset();
iforce2d 1:411d267f9d32 208 }
iforce2d 1:411d267f9d32 209
iforce2d 1:411d267f9d32 210 return armed;
iforce2d 1:411d267f9d32 211 }
iforce2d 1:411d267f9d32 212
iforce2d 1:411d267f9d32 213 bool getAutoLevelState()
iforce2d 1:411d267f9d32 214 {
iforce2d 1:411d267f9d32 215 return ppm.channels[5] > 1600;
iforce2d 1:411d267f9d32 216 }
iforce2d 1:411d267f9d32 217
iforce2d 1:411d267f9d32 218 int afterDeadband(int in)
iforce2d 1:411d267f9d32 219 {
iforce2d 1:411d267f9d32 220 int lower = 1500 - RC_DEADBAND_US;
iforce2d 1:411d267f9d32 221 int upper = 1500 + RC_DEADBAND_US;
iforce2d 1:411d267f9d32 222 if ( in < lower )
iforce2d 1:411d267f9d32 223 return in - lower;
iforce2d 1:411d267f9d32 224 if ( in > upper )
iforce2d 1:411d267f9d32 225 return in - upper;
iforce2d 1:411d267f9d32 226 return 0;
iforce2d 1:411d267f9d32 227 }
iforce2d 1:411d267f9d32 228
iforce2d 1:411d267f9d32 229 void resetPID()
iforce2d 1:411d267f9d32 230 {
iforce2d 1:411d267f9d32 231 pid_i_mem_roll = 0;
iforce2d 1:411d267f9d32 232 pid_last_roll_d_error = 0;
iforce2d 1:411d267f9d32 233 pid_i_mem_pitch = 0;
iforce2d 1:411d267f9d32 234 pid_last_pitch_d_error = 0;
iforce2d 1:411d267f9d32 235 pid_i_mem_yaw = 0;
iforce2d 1:411d267f9d32 236 pid_last_yaw_d_error = 0;
iforce2d 1:411d267f9d32 237 }
iforce2d 1:411d267f9d32 238
iforce2d 1:411d267f9d32 239 void calculatePID()
iforce2d 1:411d267f9d32 240 {
iforce2d 1:411d267f9d32 241 //Roll calculations
iforce2d 1:411d267f9d32 242 float pid_error_temp = gyro_smoothed[ROLL] - pid_roll_setpoint;
iforce2d 1:411d267f9d32 243 pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
iforce2d 1:411d267f9d32 244 if(pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
iforce2d 1:411d267f9d32 245 else if(pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;
iforce2d 1:411d267f9d32 246
iforce2d 1:411d267f9d32 247 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);
iforce2d 1:411d267f9d32 248 if(pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
iforce2d 1:411d267f9d32 249 else if(pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;
iforce2d 1:411d267f9d32 250
iforce2d 1:411d267f9d32 251 pid_last_roll_d_error = pid_error_temp;
iforce2d 1:411d267f9d32 252
iforce2d 1:411d267f9d32 253 //Pitch calculations
iforce2d 1:411d267f9d32 254 pid_error_temp = gyro_smoothed[PITCH] - pid_pitch_setpoint;
iforce2d 1:411d267f9d32 255 pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
iforce2d 1:411d267f9d32 256 if(pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
iforce2d 1:411d267f9d32 257 else if(pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;
iforce2d 1:411d267f9d32 258
iforce2d 1:411d267f9d32 259 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);
iforce2d 1:411d267f9d32 260 if(pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
iforce2d 1:411d267f9d32 261 else if(pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;
iforce2d 1:411d267f9d32 262
iforce2d 1:411d267f9d32 263 pid_last_pitch_d_error = pid_error_temp;
iforce2d 1:411d267f9d32 264
iforce2d 1:411d267f9d32 265 //Yaw calculations
iforce2d 1:411d267f9d32 266 pid_error_temp = gyro_smoothed[YAW] - pid_yaw_setpoint;
iforce2d 1:411d267f9d32 267 pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
iforce2d 1:411d267f9d32 268 if(pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
iforce2d 1:411d267f9d32 269 else if(pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;
iforce2d 1:411d267f9d32 270
iforce2d 1:411d267f9d32 271 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);
iforce2d 1:411d267f9d32 272 if(pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
iforce2d 1:411d267f9d32 273 else if(pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;
iforce2d 1:411d267f9d32 274
iforce2d 1:411d267f9d32 275 pid_last_yaw_d_error = pid_error_temp;
iforce2d 1:411d267f9d32 276 }
mbed_official 0:f76c26307f9a 277
mbed_official 0:f76c26307f9a 278 int main() {
iforce2d 1:411d267f9d32 279
iforce2d 1:411d267f9d32 280 i2c.frequency(400000);
iforce2d 1:411d267f9d32 281
iforce2d 1:411d267f9d32 282 buzzer = 1;
iforce2d 1:411d267f9d32 283 led_red = 1;
iforce2d 1:411d267f9d32 284 led_green = 1;
iforce2d 1:411d267f9d32 285
iforce2d 1:411d267f9d32 286 motorPin1 = 0;
iforce2d 1:411d267f9d32 287 motorPin2 = 0;
iforce2d 1:411d267f9d32 288 motorPin3 = 0;
iforce2d 1:411d267f9d32 289 motorPin4 = 0;
iforce2d 1:411d267f9d32 290 motorPin5 = 0;
iforce2d 1:411d267f9d32 291 motorPin6 = 0;
iforce2d 1:411d267f9d32 292
iforce2d 1:411d267f9d32 293 armingTimer.reset();
iforce2d 1:411d267f9d32 294
iforce2d 1:411d267f9d32 295 beep(50);
iforce2d 1:411d267f9d32 296
iforce2d 1:411d267f9d32 297 pc.printf("setIMURegisters\n");
iforce2d 1:411d267f9d32 298 setIMURegisters();
iforce2d 1:411d267f9d32 299
iforce2d 1:411d267f9d32 300 pc.printf("calibrateGyro\n");
iforce2d 1:411d267f9d32 301 calibrateGyro();
iforce2d 1:411d267f9d32 302
iforce2d 1:411d267f9d32 303 pc.printf("Finished gyro calibration (%f, %f, %f)\n", gyro_cal[0], gyro_cal[1], gyro_cal[2]);
iforce2d 1:411d267f9d32 304
iforce2d 1:411d267f9d32 305 timer.reset();
iforce2d 1:411d267f9d32 306 timer.start();
iforce2d 1:411d267f9d32 307
iforce2d 1:411d267f9d32 308 for (int i = 0; i < 3; i++)
iforce2d 1:411d267f9d32 309 gyro_smoothed[i] = 0;
iforce2d 1:411d267f9d32 310 angle_pitch = 0;
iforce2d 1:411d267f9d32 311 angle_roll = 0;
iforce2d 1:411d267f9d32 312
iforce2d 1:411d267f9d32 313 beep(50);
iforce2d 1:411d267f9d32 314 wait_ms(50);
iforce2d 1:411d267f9d32 315 beep(50);
iforce2d 1:411d267f9d32 316
iforce2d 1:411d267f9d32 317 bool armed = false;
iforce2d 1:411d267f9d32 318 bool armedLastTime = armed;
iforce2d 1:411d267f9d32 319
iforce2d 1:411d267f9d32 320 bool autoLevel = getAutoLevelState();
iforce2d 1:411d267f9d32 321 bool autoLevelLastTime = autoLevel;
iforce2d 1:411d267f9d32 322
iforce2d 1:411d267f9d32 323 unsigned long oneShotCounter = 0;
iforce2d 1:411d267f9d32 324
iforce2d 1:411d267f9d32 325 unsigned long loopCounter = 0;
iforce2d 1:411d267f9d32 326
iforce2d 1:411d267f9d32 327 float initialAccUptake = 0.8;
iforce2d 1:411d267f9d32 328
iforce2d 1:411d267f9d32 329 while (true) {
iforce2d 1:411d267f9d32 330
iforce2d 1:411d267f9d32 331 //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
iforce2d 1:411d267f9d32 332 //pc.printf("Accel (%f, %f)\n", angle_pitch, angle_roll);
iforce2d 1:411d267f9d32 333
iforce2d 1:411d267f9d32 334 armed = checkArmStateChange(armed);
iforce2d 1:411d267f9d32 335 led_green = ! armed;
iforce2d 1:411d267f9d32 336
iforce2d 1:411d267f9d32 337 if ( ! armedLastTime && armed )
iforce2d 1:411d267f9d32 338 resetPID();
iforce2d 1:411d267f9d32 339 if ( armed != armedLastTime ) {
iforce2d 1:411d267f9d32 340 oneShotCounter = 20;
iforce2d 1:411d267f9d32 341 beepStart();
iforce2d 1:411d267f9d32 342 }
iforce2d 1:411d267f9d32 343 armedLastTime = armed;
iforce2d 1:411d267f9d32 344
iforce2d 1:411d267f9d32 345 autoLevel = getAutoLevelState();
iforce2d 1:411d267f9d32 346 if ( autoLevel != autoLevelLastTime ) {
iforce2d 1:411d267f9d32 347 oneShotCounter = 4;
iforce2d 1:411d267f9d32 348 beepStart();
iforce2d 1:411d267f9d32 349 }
iforce2d 1:411d267f9d32 350 autoLevelLastTime = autoLevel;
iforce2d 1:411d267f9d32 351
iforce2d 1:411d267f9d32 352 if ( oneShotCounter > 0 ) {
iforce2d 1:411d267f9d32 353 if ( --oneShotCounter == 0 ) {
iforce2d 1:411d267f9d32 354 beepStop();
iforce2d 1:411d267f9d32 355 }
iforce2d 1:411d267f9d32 356 }
iforce2d 1:411d267f9d32 357
iforce2d 1:411d267f9d32 358 const float uptake = 0.3;
iforce2d 1:411d267f9d32 359 for (int i = 0; i < 3; i++)
iforce2d 1:411d267f9d32 360 gyro_smoothed[i] = (gyro_smoothed[i] * (1-uptake)) + ((gyro_raw[i] / 65.5) * uptake); //Gyro pid input is deg/sec.
iforce2d 1:411d267f9d32 361
iforce2d 1:411d267f9d32 362
iforce2d 1:411d267f9d32 363 //Gyro angle calculations
iforce2d 1:411d267f9d32 364 //0.0000611 = 1 / (250Hz / 65.5)
iforce2d 1:411d267f9d32 365 angle_pitch += gyro_raw[ROLL] * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable.
iforce2d 1:411d267f9d32 366 angle_roll += gyro_raw[PITCH] * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable.
iforce2d 1:411d267f9d32 367
iforce2d 1:411d267f9d32 368 //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
iforce2d 1:411d267f9d32 369 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.
iforce2d 1:411d267f9d32 370 angle_roll -= angle_pitch * sin(gyro_raw[YAW] * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel.
iforce2d 1:411d267f9d32 371 angle_pitch = new_angle_pitch;
iforce2d 1:411d267f9d32 372
iforce2d 1:411d267f9d32 373 //Accelerometer angle calculations
iforce2d 1:411d267f9d32 374 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.
iforce2d 1:411d267f9d32 375
iforce2d 1:411d267f9d32 376 if (abs(acc_raw[PITCH]) < acc_total_vector) { //Prevent the asin function to produce a NaN
iforce2d 1:411d267f9d32 377 angle_pitch_acc = asin((float)acc_raw[PITCH]/acc_total_vector)* 57.296; //Calculate the pitch angle.
iforce2d 1:411d267f9d32 378 }
iforce2d 1:411d267f9d32 379 if (abs(acc_raw[ROLL]) < acc_total_vector) { //Prevent the asin function to produce a NaN
iforce2d 1:411d267f9d32 380 angle_roll_acc = asin((float)acc_raw[ROLL]/acc_total_vector)* -57.296; //Calculate the roll angle.
iforce2d 1:411d267f9d32 381 }
iforce2d 1:411d267f9d32 382
iforce2d 1:411d267f9d32 383 //Place the MPU-6050 spirit level and note the values in the following two lines for calibration.
iforce2d 1:411d267f9d32 384 angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch.
iforce2d 1:411d267f9d32 385 angle_roll_acc -= 0.0; //Accelerometer calibration value for roll.
iforce2d 1:411d267f9d32 386
iforce2d 1:411d267f9d32 387 initialAccUptake *= 0.98;
iforce2d 1:411d267f9d32 388
iforce2d 1:411d267f9d32 389 float acc_uptake = 0.0004 + initialAccUptake;
iforce2d 1:411d267f9d32 390 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.
iforce2d 1:411d267f9d32 391 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.
iforce2d 1:411d267f9d32 392
iforce2d 1:411d267f9d32 393 pitch_level_adjust = angle_pitch * 15; //Calculate the pitch angle correction
iforce2d 1:411d267f9d32 394 roll_level_adjust = angle_roll * 15; //Calculate the roll angle correction
iforce2d 1:411d267f9d32 395
iforce2d 1:411d267f9d32 396 if ( ! autoLevel ){ //If the quadcopter is not in auto-level mode
iforce2d 1:411d267f9d32 397 pitch_level_adjust = 0; //Set the pitch angle correction to zero.
iforce2d 1:411d267f9d32 398 roll_level_adjust = 0; //Set the roll angle correcion to zero.
iforce2d 1:411d267f9d32 399 }
iforce2d 1:411d267f9d32 400
iforce2d 1:411d267f9d32 401
iforce2d 1:411d267f9d32 402
iforce2d 1:411d267f9d32 403 //The PID set point in degrees per second is determined by the roll receiver input.
iforce2d 1:411d267f9d32 404 //In the case of deviding by 3 the max roll rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
iforce2d 1:411d267f9d32 405 pid_roll_setpoint = afterDeadband(RC_ROLL);
iforce2d 1:411d267f9d32 406 pid_roll_setpoint -= roll_level_adjust; //Subtract the angle correction from the standardized receiver roll input value.
iforce2d 1:411d267f9d32 407 pid_roll_setpoint /= 3.0; //Divide the setpoint for the PID roll controller by 3 to get angles in degrees.
iforce2d 1:411d267f9d32 408
iforce2d 1:411d267f9d32 409 //The PID set point in degrees per second is determined by the pitch receiver input.
iforce2d 1:411d267f9d32 410 //In the case of deviding by 3 the max pitch rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
iforce2d 1:411d267f9d32 411 pid_pitch_setpoint = afterDeadband(RC_PITCH);
iforce2d 1:411d267f9d32 412 pid_pitch_setpoint -= pitch_level_adjust; //Subtract the angle correction from the standardized receiver pitch input value.
iforce2d 1:411d267f9d32 413 pid_pitch_setpoint /= 3.0; //Divide the setpoint for the PID pitch controller by 3 to get angles in degrees.
iforce2d 1:411d267f9d32 414
iforce2d 1:411d267f9d32 415 //The PID set point in degrees per second is determined by the yaw receiver input.
iforce2d 1:411d267f9d32 416 //In the case of dividing by 3 the max yaw rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
iforce2d 1:411d267f9d32 417 pid_yaw_setpoint = 0;
iforce2d 1:411d267f9d32 418 //We need a little dead band of 16us for better results.
iforce2d 1:411d267f9d32 419 if (RC_THROTTLE > 1050) { //Do not yaw when turning off the motors.
iforce2d 1:411d267f9d32 420 pid_yaw_setpoint = afterDeadband(RC_YAW) / 3.0;
iforce2d 1:411d267f9d32 421 }
iforce2d 1:411d267f9d32 422
iforce2d 1:411d267f9d32 423 calculatePID();
iforce2d 1:411d267f9d32 424
iforce2d 1:411d267f9d32 425 int pwm[6];
iforce2d 1:411d267f9d32 426
iforce2d 1:411d267f9d32 427 if (armed) { //The motors are started.
iforce2d 1:411d267f9d32 428 int throttle = RC_THROTTLE;
iforce2d 1:411d267f9d32 429 if (throttle > 1800)
iforce2d 1:411d267f9d32 430 throttle = 1800; //We need some room to keep full control at full throttle.
iforce2d 1:411d267f9d32 431 pwm[0] = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
iforce2d 1:411d267f9d32 432 pwm[1] = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
iforce2d 1:411d267f9d32 433 pwm[2] = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
iforce2d 1:411d267f9d32 434 pwm[3] = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)
iforce2d 1:411d267f9d32 435 pwm[4] = 1000;
iforce2d 1:411d267f9d32 436 pwm[5] = 1000;
iforce2d 1:411d267f9d32 437
iforce2d 1:411d267f9d32 438 for (int i = 0; i < 6; i++)
iforce2d 1:411d267f9d32 439 pwm[i] = constrain( pwm[i], 1100, 2000); //Keep the motors running.
iforce2d 1:411d267f9d32 440 }
iforce2d 1:411d267f9d32 441 else {
iforce2d 1:411d267f9d32 442 for (int i = 0; i < 6; i++)
iforce2d 1:411d267f9d32 443 pwm[i] = 1000; //If start is not 2 keep a 1000us pulse for ess-1.
iforce2d 1:411d267f9d32 444 }
iforce2d 1:411d267f9d32 445
iforce2d 1:411d267f9d32 446
iforce2d 1:411d267f9d32 447
iforce2d 1:411d267f9d32 448
iforce2d 1:411d267f9d32 449 while ( timer.read_us() < 4000 )
iforce2d 1:411d267f9d32 450 ; // wait
iforce2d 1:411d267f9d32 451
iforce2d 1:411d267f9d32 452 motorPin1 = motorPin2 = motorPin3 = motorPin4 = 1;
iforce2d 1:411d267f9d32 453 timer.reset();
iforce2d 1:411d267f9d32 454
iforce2d 1:411d267f9d32 455 readIMU();
iforce2d 1:411d267f9d32 456
iforce2d 1:411d267f9d32 457 if ( loopCounter++ % 20 == 0 ) {
iforce2d 1:411d267f9d32 458 //pc.printf("%d %d %d %d\n", pwm[0], pwm[1], pwm[2], pwm[3]);
iforce2d 1:411d267f9d32 459 pc.printf("%f %f\n", angle_roll, angle_pitch );
iforce2d 1:411d267f9d32 460 //pc.printf("%d\n", pwm[0] );
iforce2d 1:411d267f9d32 461 //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
iforce2d 1:411d267f9d32 462 //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]);
iforce2d 1:411d267f9d32 463 }
iforce2d 1:411d267f9d32 464
iforce2d 1:411d267f9d32 465 bool doneOutput = false;
iforce2d 1:411d267f9d32 466 while ( ! doneOutput ) {
iforce2d 1:411d267f9d32 467 int now = timer.read_us();
iforce2d 1:411d267f9d32 468 int c = 0;
iforce2d 1:411d267f9d32 469 if ( now >= pwm[0] ) { motorPin1 = 0; c++; }
iforce2d 1:411d267f9d32 470 if ( now >= pwm[1] ) { motorPin2 = 0; c++; }
iforce2d 1:411d267f9d32 471 if ( now >= pwm[2] ) { motorPin3 = 0; c++; }
iforce2d 1:411d267f9d32 472 if ( now >= pwm[3] ) { motorPin4 = 0; c++; }
iforce2d 1:411d267f9d32 473 if ( now >= pwm[4] ) { motorPin5 = 0; c++; }
iforce2d 1:411d267f9d32 474 if ( now >= pwm[5] ) { motorPin6 = 0; c++; }
iforce2d 1:411d267f9d32 475 doneOutput = c >= 6;
iforce2d 1:411d267f9d32 476 }
iforce2d 1:411d267f9d32 477
mbed_official 0:f76c26307f9a 478 }
iforce2d 1:411d267f9d32 479
iforce2d 1:411d267f9d32 480 /*while (true) {
iforce2d 1:411d267f9d32 481 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]);
iforce2d 1:411d267f9d32 482 }*/
iforce2d 1:411d267f9d32 483 }