Dominik Diedrich
/
drone_quado_x
code to fly a quadrocopter
main.cpp@0:b0f9c5ac0305, 2020-05-05 (annotated)
- Committer:
- DD1993
- Date:
- Tue May 05 21:11:38 2020 +0000
- Revision:
- 0:b0f9c5ac0305
initial
Who changed what in which revision?
User | Revision | Line number | New 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 | } |