code to fly a quadrocopter

Dependencies:   mbed

Committer:
DD1993
Date:
Tue May 05 21:11:38 2020 +0000
Revision:
0:b0f9c5ac0305
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DD1993 0:b0f9c5ac0305 1 #include "mbed.h"
DD1993 0:b0f9c5ac0305 2 #include "pwmdeg.h"
DD1993 0:b0f9c5ac0305 3 #include "degpwm.h"
DD1993 0:b0f9c5ac0305 4 #include "PID_roll.h"
DD1993 0:b0f9c5ac0305 5 #include "PID_pitch.h"
DD1993 0:b0f9c5ac0305 6 #include "PID_yaw.h"
DD1993 0:b0f9c5ac0305 7 #include "mpu_output.h"
DD1993 0:b0f9c5ac0305 8 #include "motor_mixer.h"
DD1993 0:b0f9c5ac0305 9
DD1993 0:b0f9c5ac0305 10 #define pitch_output
DD1993 0:b0f9c5ac0305 11
DD1993 0:b0f9c5ac0305 12 RawSerial pc_main(USBTX, USBRX);
DD1993 0:b0f9c5ac0305 13 mpu_output main_mpu_output;
DD1993 0:b0f9c5ac0305 14 pwmdeg pwmdeg_main;
DD1993 0:b0f9c5ac0305 15 Degpwm Degpwm_main;
DD1993 0:b0f9c5ac0305 16 motormixer motormixer_main;
DD1993 0:b0f9c5ac0305 17 Timer test;
DD1993 0:b0f9c5ac0305 18
DD1993 0:b0f9c5ac0305 19 //Interrupt Handler
DD1993 0:b0f9c5ac0305 20 InterruptIn roll_int(A0); //Channel 1
DD1993 0:b0f9c5ac0305 21 InterruptIn pitch_int(A1); //Channel 2
DD1993 0:b0f9c5ac0305 22 InterruptIn throttle_int(D11); //Channel 3
DD1993 0:b0f9c5ac0305 23 InterruptIn yaw_int(D12); //Channel 4
DD1993 0:b0f9c5ac0305 24
DD1993 0:b0f9c5ac0305 25 //Variablen ChannelIn
DD1993 0:b0f9c5ac0305 26 volatile uint16_t _pulsewidth_ch1;
DD1993 0:b0f9c5ac0305 27 volatile uint16_t _pulsewidth_ch2;
DD1993 0:b0f9c5ac0305 28 volatile uint16_t _pulsewidth_ch3;
DD1993 0:b0f9c5ac0305 29 volatile uint16_t _pulsewidth_ch4;
DD1993 0:b0f9c5ac0305 30
DD1993 0:b0f9c5ac0305 31 //Timer ChannelIn
DD1993 0:b0f9c5ac0305 32 Timer _t_ch1;
DD1993 0:b0f9c5ac0305 33 Timer _t_ch2;
DD1993 0:b0f9c5ac0305 34 Timer _t_ch3;
DD1993 0:b0f9c5ac0305 35 Timer _t_ch4;
DD1993 0:b0f9c5ac0305 36
DD1993 0:b0f9c5ac0305 37
DD1993 0:b0f9c5ac0305 38 //Init von PID Regler
DD1993 0:b0f9c5ac0305 39 //Beispiel: PIDClass pid_roll(AngleKp, AngleKi, RateKp, RateKd);
DD1993 0:b0f9c5ac0305 40
DD1993 0:b0f9c5ac0305 41 //Roll
DD1993 0:b0f9c5ac0305 42 PID_ROLL_Class pid_roll(4, 0.0007, 0.020, 0);
DD1993 0:b0f9c5ac0305 43
DD1993 0:b0f9c5ac0305 44 //Pitch
DD1993 0:b0f9c5ac0305 45 PID_PITCH_Class pid_pitch(4, 0.0007, 0.020, 0); //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35); //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35); //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35);
DD1993 0:b0f9c5ac0305 46
DD1993 0:b0f9c5ac0305 47 //Yaw
DD1993 0:b0f9c5ac0305 48 PID_YAW_Class pid_yaw(4, 0.000, 0.000, 0.0); //PID_YAW_Class pid_yaw(4, 0.0002, 0.015, 0.0); Stärker einstellen!!!!
DD1993 0:b0f9c5ac0305 49
DD1993 0:b0f9c5ac0305 50 float Rate = 0, dt = 10;
DD1993 0:b0f9c5ac0305 51
DD1993 0:b0f9c5ac0305 52 //Main Funktionen
DD1993 0:b0f9c5ac0305 53 void pwm_in_main();
DD1993 0:b0f9c5ac0305 54 void pitch_rc_main();
DD1993 0:b0f9c5ac0305 55 void roll_rc_main();
DD1993 0:b0f9c5ac0305 56 void yaw_rc_main();
DD1993 0:b0f9c5ac0305 57 void motors_ready_main();
DD1993 0:b0f9c5ac0305 58 void pwm_read_main();
DD1993 0:b0f9c5ac0305 59 void rise_ch1();
DD1993 0:b0f9c5ac0305 60 void fall_ch1();
DD1993 0:b0f9c5ac0305 61 void rise_ch2();
DD1993 0:b0f9c5ac0305 62 void fall_ch2();
DD1993 0:b0f9c5ac0305 63 void rise_ch3();
DD1993 0:b0f9c5ac0305 64 void fall_ch3();
DD1993 0:b0f9c5ac0305 65 void rise_ch4();
DD1993 0:b0f9c5ac0305 66 void fall_ch4();
DD1993 0:b0f9c5ac0305 67
DD1993 0:b0f9c5ac0305 68 //PwmIn Variablen
DD1993 0:b0f9c5ac0305 69 int ele_in_main = 0; //Channel 1
DD1993 0:b0f9c5ac0305 70 int ail_in_main = 0; //Channel 2
DD1993 0:b0f9c5ac0305 71 int thr_in_main = 0; //Channel 3
DD1993 0:b0f9c5ac0305 72 int rud_in_main = 0; //Channel 4
DD1993 0:b0f9c5ac0305 73
DD1993 0:b0f9c5ac0305 74
DD1993 0:b0f9c5ac0305 75 //Übergabevariablen
DD1993 0:b0f9c5ac0305 76 //Channel1
DD1993 0:b0f9c5ac0305 77 float roll_error = 0;
DD1993 0:b0f9c5ac0305 78 int roll_error_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 79
DD1993 0:b0f9c5ac0305 80 //Channel2
DD1993 0:b0f9c5ac0305 81 float pitch_error = 0;
DD1993 0:b0f9c5ac0305 82 int pitch_error_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 83
DD1993 0:b0f9c5ac0305 84 //Channel3
DD1993 0:b0f9c5ac0305 85 float yaw_error = 0;
DD1993 0:b0f9c5ac0305 86 int yaw_error_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 87
DD1993 0:b0f9c5ac0305 88 //Channel4
DD1993 0:b0f9c5ac0305 89 int motor_all_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 90
DD1993 0:b0f9c5ac0305 91
DD1993 0:b0f9c5ac0305 92
DD1993 0:b0f9c5ac0305 93 ////////////////////////////////////MAIN///////////////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 94 int main()
DD1993 0:b0f9c5ac0305 95 {
DD1993 0:b0f9c5ac0305 96 NVIC_SetPriority(EXTI3_IRQn, 0x01); //Priorität von Interrupt des Sensors dekrementieren!!!
DD1993 0:b0f9c5ac0305 97
DD1993 0:b0f9c5ac0305 98 main_mpu_output.Init(); //Sensor initalisieren
DD1993 0:b0f9c5ac0305 99 main_mpu_output.dmpDataUpdate();
DD1993 0:b0f9c5ac0305 100
DD1993 0:b0f9c5ac0305 101 //MBED_ASSERT(main_mpu_output.Init() == true);
DD1993 0:b0f9c5ac0305 102
DD1993 0:b0f9c5ac0305 103 motormixer_main.motor_all_period(2.5); //Einstellen Periode Motoren
DD1993 0:b0f9c5ac0305 104 //motormixer_main.motor_calibration(2.5); //Kalibrieren der Motoren
DD1993 0:b0f9c5ac0305 105
DD1993 0:b0f9c5ac0305 106 //ISR Funktionen RC ChannelIn
DD1993 0:b0f9c5ac0305 107 roll_int.rise(&rise_ch1);
DD1993 0:b0f9c5ac0305 108 pitch_int.rise(&rise_ch2);
DD1993 0:b0f9c5ac0305 109 throttle_int.rise(&rise_ch3);
DD1993 0:b0f9c5ac0305 110 yaw_int.rise(&rise_ch4);
DD1993 0:b0f9c5ac0305 111
DD1993 0:b0f9c5ac0305 112 while(1) {
DD1993 0:b0f9c5ac0305 113 test.start();
DD1993 0:b0f9c5ac0305 114
DD1993 0:b0f9c5ac0305 115 //Alle Channel einlesen
DD1993 0:b0f9c5ac0305 116 pwm_in_main();
DD1993 0:b0f9c5ac0305 117
DD1993 0:b0f9c5ac0305 118 //PWM Signale des RC in Grad umrechnen, Roll-Error abspeichern (danach in PWM)
DD1993 0:b0f9c5ac0305 119 roll_rc_main();
DD1993 0:b0f9c5ac0305 120
DD1993 0:b0f9c5ac0305 121 //PWM Signale des RC in Grad umrechnen, Pitch-Error abspeichern (danach in PWM)
DD1993 0:b0f9c5ac0305 122 pitch_rc_main();
DD1993 0:b0f9c5ac0305 123
DD1993 0:b0f9c5ac0305 124 //PWM Signale des RC in Grad umrechnen, Yaw-Error abspeichern (danach in PWM)
DD1993 0:b0f9c5ac0305 125 yaw_rc_main();
DD1993 0:b0f9c5ac0305 126
DD1993 0:b0f9c5ac0305 127 //Werte in Mototrmixer schreiben --> ansteuern der Motoren
DD1993 0:b0f9c5ac0305 128 motors_ready_main();
DD1993 0:b0f9c5ac0305 129
DD1993 0:b0f9c5ac0305 130 //pc_main.printf("hier\n");
DD1993 0:b0f9c5ac0305 131 test.stop();
DD1993 0:b0f9c5ac0305 132 int zeit = test.read_ms();
DD1993 0:b0f9c5ac0305 133 //pc_main.printf("zeit: %d\n", zeit);
DD1993 0:b0f9c5ac0305 134 test.reset();
DD1993 0:b0f9c5ac0305 135
DD1993 0:b0f9c5ac0305 136 }
DD1993 0:b0f9c5ac0305 137 }
DD1993 0:b0f9c5ac0305 138
DD1993 0:b0f9c5ac0305 139 ///////////////////ISR von Rise Channel 1 Roll/////////////////////////////////////
DD1993 0:b0f9c5ac0305 140 void rise_ch1()
DD1993 0:b0f9c5ac0305 141 {
DD1993 0:b0f9c5ac0305 142 _t_ch1.start();
DD1993 0:b0f9c5ac0305 143 roll_int.fall(&fall_ch1);//Interrupt Fall
DD1993 0:b0f9c5ac0305 144 }
DD1993 0:b0f9c5ac0305 145
DD1993 0:b0f9c5ac0305 146 void fall_ch1()
DD1993 0:b0f9c5ac0305 147 {
DD1993 0:b0f9c5ac0305 148 _t_ch1.stop();
DD1993 0:b0f9c5ac0305 149 if(_t_ch1.read_us()>=1000&&_t_ch1.read_us()<=2000) {
DD1993 0:b0f9c5ac0305 150 _pulsewidth_ch1 = _t_ch1.read_us();
DD1993 0:b0f9c5ac0305 151 //pc_main.printf("ch1: %d\n", _pulsewidth_ch1);
DD1993 0:b0f9c5ac0305 152 }
DD1993 0:b0f9c5ac0305 153 _t_ch1.reset();
DD1993 0:b0f9c5ac0305 154
DD1993 0:b0f9c5ac0305 155 }
DD1993 0:b0f9c5ac0305 156
DD1993 0:b0f9c5ac0305 157 ///////////////////ISR von Rise Channel 2 Pitch/////////////////////////////////////
DD1993 0:b0f9c5ac0305 158 void rise_ch2()
DD1993 0:b0f9c5ac0305 159 {
DD1993 0:b0f9c5ac0305 160 _t_ch2.start();
DD1993 0:b0f9c5ac0305 161 pitch_int.fall(&fall_ch2);//Interrupt Fall
DD1993 0:b0f9c5ac0305 162
DD1993 0:b0f9c5ac0305 163 }
DD1993 0:b0f9c5ac0305 164
DD1993 0:b0f9c5ac0305 165 void fall_ch2()
DD1993 0:b0f9c5ac0305 166 {
DD1993 0:b0f9c5ac0305 167 _t_ch2.stop();
DD1993 0:b0f9c5ac0305 168 if(_t_ch2.read_us()>=1000&&_t_ch2.read_us()<=2000) {
DD1993 0:b0f9c5ac0305 169 _pulsewidth_ch2 = _t_ch2.read_us();
DD1993 0:b0f9c5ac0305 170 //pc_main.printf("ch1: %d\n", _pulsewidth_ch2);
DD1993 0:b0f9c5ac0305 171 }
DD1993 0:b0f9c5ac0305 172 _t_ch2.reset();
DD1993 0:b0f9c5ac0305 173
DD1993 0:b0f9c5ac0305 174 }
DD1993 0:b0f9c5ac0305 175
DD1993 0:b0f9c5ac0305 176 ///////////////////ISR von Rise Channel 3 Throttle/////////////////////////////////////
DD1993 0:b0f9c5ac0305 177 void rise_ch3()
DD1993 0:b0f9c5ac0305 178 {
DD1993 0:b0f9c5ac0305 179 _t_ch3.start();
DD1993 0:b0f9c5ac0305 180 throttle_int.fall(&fall_ch3);//Interrupt Fall
DD1993 0:b0f9c5ac0305 181 }
DD1993 0:b0f9c5ac0305 182
DD1993 0:b0f9c5ac0305 183 void fall_ch3()
DD1993 0:b0f9c5ac0305 184 {
DD1993 0:b0f9c5ac0305 185 _t_ch3.stop();
DD1993 0:b0f9c5ac0305 186 if(_t_ch3.read_us()>=1000&&_t_ch3.read_us()<=2000) {
DD1993 0:b0f9c5ac0305 187 _pulsewidth_ch3 = _t_ch3.read_us();
DD1993 0:b0f9c5ac0305 188 //pc_main.printf("ch1: %d\n", _pulsewidth_ch1);
DD1993 0:b0f9c5ac0305 189 }
DD1993 0:b0f9c5ac0305 190 _t_ch3.reset();
DD1993 0:b0f9c5ac0305 191
DD1993 0:b0f9c5ac0305 192 }
DD1993 0:b0f9c5ac0305 193
DD1993 0:b0f9c5ac0305 194 ///////////////////ISR von Rise Channel 4 YAW/////////////////////////////////////
DD1993 0:b0f9c5ac0305 195 void rise_ch4()
DD1993 0:b0f9c5ac0305 196 {
DD1993 0:b0f9c5ac0305 197 _t_ch4.start();
DD1993 0:b0f9c5ac0305 198 yaw_int.fall(&fall_ch4);//Interrupt Fall
DD1993 0:b0f9c5ac0305 199
DD1993 0:b0f9c5ac0305 200 }
DD1993 0:b0f9c5ac0305 201
DD1993 0:b0f9c5ac0305 202 void fall_ch4()
DD1993 0:b0f9c5ac0305 203 {
DD1993 0:b0f9c5ac0305 204 _t_ch4.stop();
DD1993 0:b0f9c5ac0305 205 if(_t_ch4.read_us()>=1000&&_t_ch4.read_us()<=2000) {
DD1993 0:b0f9c5ac0305 206 _pulsewidth_ch4 = _t_ch4.read_us();
DD1993 0:b0f9c5ac0305 207 //pc_main.printf("ch4: %d\n", _pulsewidth_ch4);
DD1993 0:b0f9c5ac0305 208 }
DD1993 0:b0f9c5ac0305 209 _t_ch4.reset();
DD1993 0:b0f9c5ac0305 210
DD1993 0:b0f9c5ac0305 211 }
DD1993 0:b0f9c5ac0305 212
DD1993 0:b0f9c5ac0305 213
DD1993 0:b0f9c5ac0305 214 ///////PWM In abspeichern///////////////////////////////
DD1993 0:b0f9c5ac0305 215
DD1993 0:b0f9c5ac0305 216 void pwm_in_main()
DD1993 0:b0f9c5ac0305 217 {
DD1993 0:b0f9c5ac0305 218 ail_in_main = _pulsewidth_ch1;
DD1993 0:b0f9c5ac0305 219 ele_in_main = _pulsewidth_ch2;
DD1993 0:b0f9c5ac0305 220 thr_in_main = _pulsewidth_ch3;
DD1993 0:b0f9c5ac0305 221 rud_in_main = _pulsewidth_ch4;
DD1993 0:b0f9c5ac0305 222
DD1993 0:b0f9c5ac0305 223 //pc_main.printf("throttle: %d|pitch: %d|roll: %d|rud: %d \n", thr_in_main, ele_in_main, ail_in_main, rud_in_main);
DD1993 0:b0f9c5ac0305 224
DD1993 0:b0f9c5ac0305 225 }
DD1993 0:b0f9c5ac0305 226
DD1993 0:b0f9c5ac0305 227 ///////////Roll Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 228 void roll_rc_main()
DD1993 0:b0f9c5ac0305 229 {
DD1993 0:b0f9c5ac0305 230 //Roll-Error abspeichern (in PWM)
DD1993 0:b0f9c5ac0305 231 float Setpoint_Roll = pwmdeg_main.roll(ail_in_main), CurrentPosition_Roll = main_mpu_output.sensor_roll();
DD1993 0:b0f9c5ac0305 232 roll_error = pid_roll.update_roll(Setpoint_Roll, CurrentPosition_Roll, Rate, dt);
DD1993 0:b0f9c5ac0305 233 //pc_main.printf("Output pitch error: %d\n", pitch_error);
DD1993 0:b0f9c5ac0305 234 roll_error_pwm_ready = Degpwm_main.roll_pwm(roll_error);
DD1993 0:b0f9c5ac0305 235 //pc_main.printf("Output roll error pwm: %d\n", roll_error_pwm_ready);
DD1993 0:b0f9c5ac0305 236 }
DD1993 0:b0f9c5ac0305 237
DD1993 0:b0f9c5ac0305 238 ///////////Pitch Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 239 void pitch_rc_main()
DD1993 0:b0f9c5ac0305 240 {
DD1993 0:b0f9c5ac0305 241 float Setpoint_Pitch = pwmdeg_main.pitch_forward(ele_in_main), CurrentPosition_Pitch = main_mpu_output.sensor_pitch();
DD1993 0:b0f9c5ac0305 242 pitch_error = pid_pitch.update_pitch(Setpoint_Pitch, CurrentPosition_Pitch, Rate, dt);
DD1993 0:b0f9c5ac0305 243 //pc_main.printf("Output pitch error: %f\n", pitch_error);
DD1993 0:b0f9c5ac0305 244 pitch_error_pwm_ready = Degpwm_main.pitch_pwm(pitch_error);
DD1993 0:b0f9c5ac0305 245 //pc_main.printf("Output pitch error pwm: %d\n", pitch_error_pwm_ready);
DD1993 0:b0f9c5ac0305 246 #ifdef pitch_output
DD1993 0:b0f9c5ac0305 247 pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output pitch error pwm: %d|pitch error: %f\n", Setpoint_Pitch, CurrentPosition_Pitch, pitch_error_pwm_ready, pitch_error);
DD1993 0:b0f9c5ac0305 248 #endif
DD1993 0:b0f9c5ac0305 249
DD1993 0:b0f9c5ac0305 250 }
DD1993 0:b0f9c5ac0305 251
DD1993 0:b0f9c5ac0305 252
DD1993 0:b0f9c5ac0305 253 ///////////Yaw Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 254 void yaw_rc_main()
DD1993 0:b0f9c5ac0305 255 {
DD1993 0:b0f9c5ac0305 256 //Yaw Error abspeichern (in PWM)
DD1993 0:b0f9c5ac0305 257 float SetAccel_Yaw = pwmdeg_main.yaw(rud_in_main), CurrentAccel_Yaw = main_mpu_output.sensor_yaw();
DD1993 0:b0f9c5ac0305 258 yaw_error = pid_yaw.update_yaw(SetAccel_Yaw, CurrentAccel_Yaw, Rate, dt);
DD1993 0:b0f9c5ac0305 259 yaw_error_pwm_ready = Degpwm_main.yaw_pwm(yaw_error);
DD1993 0:b0f9c5ac0305 260 //pc_main.printf("Output yaw error pwm: %d\n", yaw_error_pwm_ready);
DD1993 0:b0f9c5ac0305 261 //pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output yaw error pwm: %d|yaw error: %f\n", SetAccel_Yaw, CurrentAccel_Yaw, yaw_error_pwm_ready, yaw_error);
DD1993 0:b0f9c5ac0305 262
DD1993 0:b0f9c5ac0305 263 }
DD1993 0:b0f9c5ac0305 264
DD1993 0:b0f9c5ac0305 265 ///////////Übergabe an Motormixer///////////////////////////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 266 void motors_ready_main()
DD1993 0:b0f9c5ac0305 267 {
DD1993 0:b0f9c5ac0305 268 //yaw_error_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 269 //Throttle abspeichern (in PWM)
DD1993 0:b0f9c5ac0305 270 if(thr_in_main > 1111) {
DD1993 0:b0f9c5ac0305 271
DD1993 0:b0f9c5ac0305 272 motor_all_pwm_ready = thr_in_main;
DD1993 0:b0f9c5ac0305 273
DD1993 0:b0f9c5ac0305 274 //PWM Werte in Motormixer und in fertige Motoren in Variablen speichern
DD1993 0:b0f9c5ac0305 275 //Motor1
DD1993 0:b0f9c5ac0305 276 motormixer_main.motor1_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
DD1993 0:b0f9c5ac0305 277 //Motor2
DD1993 0:b0f9c5ac0305 278 motormixer_main.motor2_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
DD1993 0:b0f9c5ac0305 279 //Motor3
DD1993 0:b0f9c5ac0305 280 motormixer_main.motor3_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
DD1993 0:b0f9c5ac0305 281 //Motor4
DD1993 0:b0f9c5ac0305 282 motormixer_main.motor4_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
DD1993 0:b0f9c5ac0305 283 }
DD1993 0:b0f9c5ac0305 284
DD1993 0:b0f9c5ac0305 285 else {
DD1993 0:b0f9c5ac0305 286 //pc_main.printf("throttle: %d\n", throttle_in.pulsewidth());
DD1993 0:b0f9c5ac0305 287 motormixer_main.motor_all_off();
DD1993 0:b0f9c5ac0305 288 }
DD1993 0:b0f9c5ac0305 289
DD1993 0:b0f9c5ac0305 290 }