Matthias Grob
My fully self designed first stable working Quadrocopter Software.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* X- Configuration +-Configuration 00002 m0 m3 m1 -- > 00003 \ / | / \ / 00004 / \ m2-------m0 V | 00005 m1 m2 | \ 00006 m3 PITCH ROLL*/ 00007 #include "mbed.h" 00008 #include "LED.h" // LEDs framework for blinking ;) 00009 #include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe) 00010 00011 #include "IMU_10DOF.h" // Complete IMU class for 10DOF-Board (L3G4200D, ADXL345, HMC5883, BMP085) 00012 #include "RC_Channel.h" // RemoteControl Channels with PPM 00013 #include "PID.h" // PID Library (slim, self written) 00014 #include "Servo.h" // Motor PPM using any DigitalOut Pin 00015 00016 #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) 00017 #define INTEGRAL_MAX 300 // maximal output offset that can result from integrating errors 00018 #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for 00019 #define YAWSPEED 1.0 // maximal speed of yaw rotation in degree per Rate 00020 #define AILERON 0 // RC 00021 #define ELEVATOR 1 00022 #define RUDDER 2 00023 #define THROTTLE 3 00024 #define CHANNEL8 4 00025 #define CHANNEL7 5 00026 #define CHANNEL6 6 00027 #define ROLL 0 // Axes 00028 #define PITCH 1 00029 #define YAW 2 00030 00031 #define SQRT2 0.7071067811865 00032 00033 //#define CONSTRAIN(VAL,LIMIT) ((VAL)<(-LIMIT)?(-LIMIT):((VAL)>(LIMIT)?(LIMIT):(VAL))) 00034 00035 bool armed = false; // is for security (when false no motor rotates any more) 00036 bool debug = true; // shows if we want output for the computer 00037 bool RC_present = false; // shows if an RC is present 00038 float P_R = 2.5, I_R = 3.7, D_R = 0; 00039 float P_A = 1.865, I_A = 1.765, D_A = 0; 00040 //float P = 13.16, I = 8, D = 2.73; // PID values 00041 float PY = 3.2, IY = 0, DY = 0; 00042 //float PY = 5.37, IY = 0, DY = 3; // PID values for Yaw 00043 float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC 00044 float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send 00045 //float * command_pointer = &D; // TODO: pointer to varible that's going to be changed by UART command 00046 00047 /*float max[3] = {-10000,-10000,-10000}; 00048 float min[3] = {10000,10000,10000};*/ 00049 00050 LED LEDs; 00051 PC pc(USBTX, USBRX, 921600); // USB 00052 //PC pc(p9, p10, 115200); // Bluetooth 00053 IMU_10DOF IMU(p28, p27); 00054 RC_Channel RC[] = {RC_Channel(p8,1), RC_Channel(p7,2), RC_Channel(p5,4), RC_Channel(p6,3), RC_Channel(p15,2), RC_Channel(p16,4), RC_Channel(p17,3)}; // no p19/p20 ! 00055 PID Controller_Rate[] = {PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw 00056 PID Controller_Angle[] = {PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(0, 0, 0, INTEGRAL_MAX)}; 00057 Servo ESC[] = {Servo(p21,PPM_FREQU), Servo(p22,PPM_FREQU), Servo(p23,PPM_FREQU), Servo(p24,PPM_FREQU)}; // use any DigitalOit Pin 00058 00059 extern "C" void mbed_reset(); 00060 00061 void executer() { 00062 char command = pc.getc(); 00063 if (command == 'X') 00064 mbed_reset(); 00065 if (command == '-') 00066 debug = !debug; 00067 if (command == 'A') { 00068 IMU.Acc.calibrate(100,0.05); 00069 pc.printf("\r\n***A***%.3f,%.3f,%.3f***\r\n", IMU.Acc.offset[ROLL], IMU.Acc.offset[PITCH], IMU.Acc.offset[YAW]); 00070 wait(10); 00071 } 00072 if (command == 'C') { 00073 IMU.Comp.calibrate(60); 00074 pc.printf("\r\n***C***%.3f,%.3f,%.3f***\r\n", IMU.Comp.offset[ROLL], IMU.Comp.offset[PITCH], IMU.Comp.offset[YAW]); 00075 wait(20); 00076 } 00077 00078 pc.putc(command); 00079 LEDs.tilt(2); 00080 } 00081 00082 int main() { 00083 pc.attach(&executer); 00084 while(1) { 00085 // IMU 00086 IMU.readAngles(); 00087 //IMU.readAltitude(); // TODO: reading altitude takes much more time than the angles -> don't do this in your fast loop, Ticker? 00088 //pc.printf("%.1f,%.1f,%.1f,%.1f'C,%.1fhPa,%.1fmaS,%.5fs,%.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.temperature, IMU.pressure, IMU.altitude, IMU.dt, IMU.dt_sensors); // Output for Python 00089 00090 // Arming / disarming 00091 RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe 00092 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) { 00093 armed = true; 00094 RC_angle[YAW] = IMU.angle[YAW]; 00095 } 00096 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) { 00097 armed = false; 00098 } 00099 00100 // Setting PID Values from auxiliary RC channels 00101 //if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000) 00102 // P_R = 0 + (((float)RC[CHANNEL8].read()) * 3 / 1000); 00103 /*if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000) 00104 I_R = 0 + (((float)RC[CHANNEL7].read()) * 12 / 1000);*/ 00105 for(int i=0;i<3;i++) 00106 Controller_Angle[i].setPID(P_A,I_A,D_A); 00107 for(int i=0;i<2;i++) 00108 Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller 00109 Controller_Rate[YAW].setPID(PY,IY,DY); 00110 00111 // RC Angle ROLL-PITCH-Part 00112 for(int i=0;i<2;i++) { // calculate new angle we want the QC to have 00113 if (RC_present) 00114 RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0; 00115 else 00116 RC_angle[i] = 0; 00117 } 00118 00119 // RC Angle YAW-Part 00120 if (RC_present && RC[THROTTLE].read() > 20) 00121 RC_angle[YAW] -= (RC[RUDDER].read()-500)*YAWSPEED/500; 00122 00123 float RC_yaw_adding; // temporary variable to take the desired yaw adjustment 00124 if (RC_present && RC[THROTTLE].read() > 20) 00125 RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; 00126 else 00127 RC_yaw_adding = 0; 00128 00129 RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding < -180 ? RC_angle[YAW] + 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; 00130 RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding > 180 ? RC_angle[YAW] - 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; 00131 00132 /*float RC_yaw_adding; // temporary variable to take the desired yaw adjustment 00133 if (RC_present && RC[THROTTLE].read() > 20) 00134 RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; 00135 else 00136 RC_yaw_adding = 0; 00137 00138 while(RC_angle[YAW] + RC_yaw_adding < -180 || RC_angle[YAW] + RC_yaw_adding > 180) { // make shure it's in the cycle -180 to 180 00139 if(RC_angle[YAW] + RC_yaw_adding < -180) 00140 RC_yaw_adding += 360; 00141 if(RC_angle[YAW] + RC_yaw_adding > 180) 00142 RC_yaw_adding -= 360; 00143 } 00144 RC_angle[YAW] += RC_yaw_adding; // the yaw angle is integrated from stick input*/ 00145 00146 // Controlling 00147 for(int i=0;i<2;i++) { 00148 Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying 00149 Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values and get his advice to correct 00150 } 00151 Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying 00152 if (RC[THROTTLE].read() > 20) 00153 Controller_Rate[2].compute(-(RC[2].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct 00154 else 00155 Controller_Rate[2].compute(0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct 00156 /*for(int i=0;i<3;i++) { 00157 Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying 00158 Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct 00159 Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying 00160 Controller_Rate[i].compute(-Controller_Angle[i].Value, IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values and get his advice to correct 00161 }*/ 00162 00163 // OLD Controlling 00164 /*for(int i=0;i<2;i++) { 00165 Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying 00166 Controller[i].compute(RC_angle[i], IMU.angle[i], IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values for D and angle for P,I and get his advice to correct 00167 } 00168 Controller[YAW].setIntegrate(armed); // same for YAW 00169 if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180) // for YAW a special calculation because of range -180 to 180 00170 if (RC_angle[YAW] > IMU.angle[YAW]) 00171 Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]); 00172 else 00173 Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]); 00174 else 00175 Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);*/ 00176 00177 // Mixing 00178 /*Motor_speed[2] = RC[THROTTLE].read() + Controller_Rate[PITCH].Value; // PITCH in direction + Configuration 00179 Motor_speed[0] = RC[THROTTLE].read() - Controller_Rate[PITCH].Value; // PITCH against direction 00180 Motor_speed[1] = RC[THROTTLE].read() + Controller_Rate[ROLL].Value; // ROLL in direction 00181 Motor_speed[3] = RC[THROTTLE].read() - Controller_Rate[ROLL].Value; // ROLL against direction*/ 00182 00183 Motor_speed[0] = RC[THROTTLE].read() +SQRT2*Controller_Rate[PITCH].Value +SQRT2*Controller_Rate[ROLL].Value; // PITCH in direction X Configuration 00184 Motor_speed[1] = RC[THROTTLE].read() +SQRT2*Controller_Rate[PITCH].Value -SQRT2*Controller_Rate[ROLL].Value; // PITCH against direction 00185 Motor_speed[2] = RC[THROTTLE].read() -SQRT2*Controller_Rate[PITCH].Value -SQRT2*Controller_Rate[ROLL].Value; // ROLL in direction 00186 Motor_speed[3] = RC[THROTTLE].read() -SQRT2*Controller_Rate[PITCH].Value +SQRT2*Controller_Rate[ROLL].Value; // ROLL against direction 00187 00188 Motor_speed[0] -= Controller_Rate[YAW].Value; 00189 Motor_speed[2] -= Controller_Rate[YAW].Value; 00190 Motor_speed[3] += Controller_Rate[YAW].Value; 00191 Motor_speed[1] += Controller_Rate[YAW].Value; 00192 00193 if (armed) // for SECURITY! 00194 { 00195 debug = false; 00196 // PITCH 00197 //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50; 00198 //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50; 00199 // ROLL 00200 //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50; 00201 //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50; 00202 for(int i=0;i<4;i++) // Set new motorspeeds 00203 ESC[i] = (int)Motor_speed[i]>50 ? (int)Motor_speed[i] : 50; 00204 00205 } else { 00206 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed 00207 ESC[i] = 0; 00208 } 00209 00210 if (debug) { 00211 //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", IMU.Acc.data[0], IMU.Acc.data[1], IMU.Acc.data[2], D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt); 00212 //MAIN OUTPUT pc.printf("%d,%.1f,%.1f,%.1f,%.3f,%.3f,%.3f,%.2f,%.2f\r\n", armed, IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW], Controller[ROLL].Value, Controller[PITCH].Value, Controller[YAW].Value, P, D); // RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read() 00213 //pc.printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", armed, P, PY, D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt); 00214 //pc.printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", armed, P, PY, D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt); 00215 //pc.printf("%+.3f,%+.3f,%+.3f,%+.3f,%+.3f,%+.3f,%.5f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.Sensor.data_gyro[0], IMU.Sensor.data_gyro[1], IMU.Sensor.data_gyro[2], IMU.dt); 00216 pc.printf("$STATE,%d,%.3f\r\n", armed, IMU.dt); 00217 pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read()); 00218 pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW]); 00219 pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_acc[ROLL], IMU.Sensor.data_acc[PITCH], IMU.Sensor.data_acc[YAW]); 00220 pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]); 00221 pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]); 00222 pc.printf("$CONT,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Rate[ROLL].Value, Controller_Rate[PITCH].Value, Controller_Rate[YAW].Value, P_R, I_R, D_R); 00223 pc.printf("$MOT,%d,%d,%d,%d\r\n", (int)Motor_speed[0], (int)Motor_speed[1], (int)Motor_speed[2], (int)Motor_speed[3]); 00224 /*for (int i=0;i<3;i++) { 00225 min[i] = IMU.Sensor.data_gyro[i]<min[i] ? IMU.Sensor.data_gyro[i] : min[i]; 00226 max[i] = IMU.Sensor.data_gyro[i]>max[i] ? IMU.Sensor.data_gyro[i] : max[i]; 00227 }*/ 00228 //pc.printf("%.5f\r\n", IMU.dt); 00229 //pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", IMU.Sensor.raw_gyro[ROLL], IMU.Sensor.raw_gyro[PITCH], IMU.Sensor.raw_gyro[YAW], min[0], min[1], min[2], max[0], max[1], max[2]); 00230 //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW], min[0], min[1], min[2], max[0], max[1], max[2]); 00231 //pc.printf("%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW]); 00232 00233 // SimPlot output 00234 /*int16_t sendvalue[4]; //Buffer to hold the packet, note it is 16bit data type 00235 sendvalue[0] = (int16_t) IMU.Sensor.data_gyro[ROLL]; //Channel 1 data. 16bit signed integer 00236 sendvalue[1] = (int16_t) IMU.Sensor.data_gyro[PITCH]; //Channel 2 data. 16bit signed integer 00237 sendvalue[2] = (int16_t) IMU.Sensor.data_gyro[YAW]; //Channel 3 data. 16bit signed integer 00238 sendvalue[3] = (int16_t) 0; //Channel 4 data. 16bit signed integer 00239 00240 pc.putc(0xAB); // header 00241 pc.putc(0xCD); 00242 pc.putc(0x08); // size LSB 00243 pc.putc(0x00); // size MSB 00244 for(int i=0; i<4; i++) { 00245 pc.putc((char)sendvalue[i]); // LSB 00246 pc.putc((char)(sendvalue[i] >> 8)); // MSB 00247 }*/ 00248 00249 wait(0.04); 00250 } 00251 00252 LEDs.rollnext(); 00253 } 00254 }
Generated on Thu Jul 14 2022 18:09:09 by
1.7.2