NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
main.cpp
00001 #include "mbed.h" // Standard Library 00002 #include "LED.h" // LEDs framework for blinking ;) 00003 #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) 00004 #include "MPU6050.h" // Combined Gyro and Acc 00005 //#include "L3G4200D.h" // Gyro (Gyroscope) 00006 //#include "ADXL345.h" // Acc (Accelerometer) 00007 //#include "HMC5883.h" // Comp (Compass) 00008 //#include "BMP085_old.h" // Alt (Altitude sensor) 00009 #include "RC_Channel.h" // RemoteControl Channels with PPM 00010 #include "Servo_PWM.h" // Motor PPM using PwmOut 00011 #include "PID.h" // PID Library (slim, self written) 00012 #include "IMU_Filter.h" // Class to calculate position angles 00013 #include "Mixer.h" // Class to calculate motorspeeds from Angles, Regulation and RC-Signals 00014 00015 #define RATE 0.002 // speed of the interrupt for Sensors and PID 00016 #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) 00017 #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for 00018 #define YAWSPEED 0.2 // maximal speed of yaw rotation in degree per Rate 00019 #define INTEGRAL_MAX 300 00020 00021 // RC 00022 #define AILERON 0 00023 #define ELEVATOR 1 00024 #define RUDDER 2 00025 #define THROTTLE 3 00026 // Axes 00027 #define ROLL 0 00028 #define PITCH 1 00029 #define YAW 2 00030 00031 #define PC_CONNECTED // decomment if you want to debug per USB/Bluetooth and your PC 00032 00033 // Global variables 00034 bool armed = false; // this variable is for security (when false no motor rotates any more) 00035 bool RC_present = false; // this variable shows if an RC is present 00036 float dt = 0; 00037 float time_for_dt = 0; 00038 float dt_read_sensors = 0; 00039 float time_read_sensors = 0; 00040 float controller_value[] = {0,0,0}; // The calculated answer form the Controller 00041 float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC 00042 float RC_yaw_adding; // temporary variable to take the desired yaw adjustment 00043 00044 float P = 4.0; // PID values 00045 float I = 0; 00046 float D = 0.1; 00047 00048 float PY = 0; // PID values for YAW 00049 float IY = 0; 00050 float DY = 0; 00051 00052 Timer GlobalTimer; // global time to calculate processing speed 00053 Ticker Dutycycler; // timecontrolled interrupt for exact timed control loop 00054 00055 // Initialisation of hardware (see includes for more info) 00056 LED LEDs; 00057 #ifdef PC_CONNECTED 00058 PC pc(USBTX, USBRX, 115200); // USB 00059 //PC pc(p9, p10, 115200); // Bluetooth 00060 #endif 00061 MPU6050 Sensor(p28, p27); 00062 //L3G4200D Gyro(p28, p27); 00063 //ADXL345 Acc(p28, p27); 00064 //HMC5883 Comp(p28, p27); 00065 //BMP085_old Alt(p28, p27); 00066 RC_Channel RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)}; // no p19/p20 ! 00067 Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed! 00068 IMU_Filter IMU; // (don't write () after constructor for no arguments!) 00069 Mixer MIX(1); // 0 for +-Formation, 1 for X-Formation 00070 PID Controller[] = {PID(P, I, D, INTEGRAL_MAX), PID(P, I, D, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw 00071 00072 void dutycycle() { // method which is called by the Ticker Dutycycler every RATE seconds 00073 time_read_sensors = GlobalTimer.read(); // start time measure for sensors 00074 00075 // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes 00076 Sensor.read(); 00077 //Gyro.read(); 00078 //Acc.read(); 00079 //Comp.read(); // TODO: not every loop every sensor? altitude not that important 00080 //Alt.Update(); // TODO needs very long to read because of waits 00081 00082 //pc.printf("%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f\r\n", Gyro.data[0], Gyro.data[1], Gyro.data[2], Acc.data[0], Acc.data[1], Acc.data[2]); 00083 00084 00085 00086 // meassure dt for the filter 00087 dt = GlobalTimer.read() - time_for_dt; // time in us since last loop 00088 time_for_dt = GlobalTimer.read(); // set new time for next measurement 00089 00090 IMU.compute(dt, Sensor.data_gyro, Sensor.data_acc); 00091 //IMU.compute(dt, Gyro.data, Acc.data); 00092 //pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors); 00093 00094 if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100) 00095 RC_present = false; 00096 else 00097 RC_present = true; 00098 00099 // Arming / disarming 00100 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) { 00101 armed = true; 00102 RC_angle[YAW] = IMU.angle[YAW]; 00103 } 00104 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) { 00105 armed = false; 00106 } 00107 00108 // RC Angle ROLL-PITCH-Part 00109 for(int i=0;i<2;i++) { // calculate new angle we want the QC to have 00110 if (RC_present) 00111 RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0; 00112 else 00113 RC_angle[i] = 0; 00114 } 00115 00116 // RC Angle YAW-Part 00117 if (RC_present && RC[THROTTLE].read() > 20) 00118 RC_yaw_adding = (RC[RUDDER].read()-500)*YAWSPEED/500; 00119 else 00120 RC_yaw_adding = 0; 00121 00122 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 00123 if(RC_angle[YAW] + RC_yaw_adding < -180) 00124 RC_yaw_adding += 360; 00125 if(RC_angle[YAW] + RC_yaw_adding > 180) 00126 RC_yaw_adding -= 360; 00127 } 00128 RC_angle[YAW] += RC_yaw_adding; // for yaw angle it's integrated 00129 00130 // PID controlling 00131 for(int i=0;i<2;i++) { 00132 Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying 00133 controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct 00134 } 00135 Controller[YAW].setIntegrate(armed); // same for YAW 00136 if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180) // for YAW a special calculation because of range -180 to 180 00137 if (RC_angle[YAW] > IMU.angle[YAW]) 00138 controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW]); 00139 else 00140 controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW]); 00141 else 00142 controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW]); 00143 00144 if (armed) // for SECURITY! 00145 { 00146 MIX.compute(RC[THROTTLE].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output 00147 for(int i=0;i<4;i++) // Set new motorspeeds 00148 ESC[i] = (int)MIX.Motor_speed[i]; 00149 00150 } else { 00151 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed 00152 ESC[i] = 0; 00153 } 00154 /*pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", 00155 armed, 00156 dt, 00157 dt_read_sensors, 00158 IMU.angle[ROLL], 00159 IMU.angle[PITCH], 00160 IMU.angle[YAW], 00161 RC_angle[ROLL], 00162 RC_angle[PITCH], 00163 RC_angle[YAW], 00164 controller_value[ROLL], 00165 controller_value[PITCH], 00166 controller_value[YAW], 00167 MIX.Motor_speed[0], 00168 MIX.Motor_speed[1], 00169 MIX.Motor_speed[2], 00170 MIX.Motor_speed[3]);*/ 00171 00172 pc.printf("%f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", 00173 IMU.angle[ROLL], 00174 IMU.angle[PITCH], 00175 IMU.angle[YAW], 00176 Sensor.data_gyro[0], 00177 Sensor.data_gyro[1], 00178 Sensor.data_gyro[2], 00179 Sensor.data_acc[0], 00180 Sensor.data_acc[1], 00181 Sensor.data_acc[2], 00182 controller_value[ROLL], 00183 controller_value[PITCH], 00184 controller_value[YAW], 00185 MIX.Motor_speed[0], 00186 MIX.Motor_speed[1], 00187 MIX.Motor_speed[2], 00188 MIX.Motor_speed[3]); 00189 00190 dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time for loop 00191 } 00192 00193 void commandexecuter(char* command) { // take new PID values on the fly 00194 if (command[0] == 'p') 00195 if (command[1] == 'y') 00196 PY = atof(&command[2]); 00197 else 00198 P = atof(&command[1]); 00199 if (command[0] == 'i') 00200 if (command[1] == 'y') 00201 IY = atof(&command[2]); 00202 else 00203 I = atof(&command[1]); 00204 if (command[0] == 'd') 00205 if (command[1] == 'y') 00206 DY = atof(&command[2]); 00207 else 00208 D = atof(&command[1]); 00209 for(int i=0;i<2;i++) { 00210 Controller[i].setPID(P,I,D); // give the controller the new PID values 00211 } 00212 Controller[YAW].setPID(PY,IY,DY); // give the controller the new PID values 00213 } 00214 00215 int main() { // main programm for initialisation and debug output 00216 NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished) 00217 00218 #ifdef PC_CONNECTED 00219 // init screen 00220 pc.locate(10,5); 00221 pc.printf("Flybed v0.2"); 00222 #endif 00223 LEDs.roll(2); 00224 00225 Sensor.calibrate(50, 0.02); 00226 //Gyro.calibrate(50, 0.02); 00227 //Acc.calibrate(50, 0.02); 00228 00229 // Start! 00230 GlobalTimer.start(); 00231 Dutycycler.attach(&dutycycle, RATE); // start to process all RATE seconds 00232 00233 while(1) { 00234 #ifdef PC_CONNECTED 00235 if (pc.readable()) // Get Serial input (polled because interrupts disturb I2C) 00236 pc.readcommand(&commandexecuter); 00237 //pc.printf("%f %f %f %f %f %f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); // For live plot in MATLAB of IMU 00238 //pc.printf("%f,%f,%f,%f,%f,%f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); 00239 #if 0 //pc.cls(); 00240 pc.locate(20,0); // PC output 00241 pc.printf("dt:%3.5fs dt_sensors:%3.5fs Altitude:%6.1fm ", dt, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); 00242 pc.locate(5,1); 00243 if(armed) 00244 pc.printf("ARMED!!!!!!!!!!!!!"); 00245 else 00246 pc.printf("DIS_ARMED "); 00247 pc.locate(5,3); 00248 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]); 00249 pc.locate(5,4); 00250 pc.printf("q0:%6.1f q1:%6.1f q2:%6.1f q3:%6.1f ", IMU.q0, IMU.q1, IMU.q2, IMU.q3); 00251 pc.locate(5,5); 00252 pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]); 00253 pc.locate(5,6); 00254 pc.printf("Acc.data: X:%6.1f Y:%6.1f Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]); 00255 00256 pc.locate(5,8); 00257 pc.printf("P :%6.1f I :%6.1f D :%6.1f ", P, I, D); 00258 pc.locate(5,9); 00259 pc.printf("PY:%6.1f IY:%6.1f DY:%6.1f ", PY, IY, DY); 00260 00261 pc.locate(5,11); 00262 pc.printf("PID Result:"); 00263 for(int i=0;i<3;i++) 00264 pc.printf(" %d: %6.1f", i, controller_value[i]); 00265 pc.locate(5,14); 00266 pc.printf("RC angle: roll: %f pitch: %f yaw: %f ", RC_angle[0], RC_angle[1], RC_angle[2]); 00267 pc.locate(5,16); 00268 pc.printf("Motor: 0:%d 1:%d 2:%d 3:%d ", (int)MIX.Motor_speed[0], (int)MIX.Motor_speed[1], (int)MIX.Motor_speed[2], (int)MIX.Motor_speed[3]); 00269 00270 // RC 00271 pc.locate(10,19); 00272 pc.printf("RC0: %4d RC1: %4d RC2: %4d RC3: %4d ", RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read()); 00273 00274 pc.locate(10,21); 00275 pc.printf("Commandline: %s ", pc.command); 00276 #endif 00277 #endif 00278 if(armed){ 00279 LEDs.rollnext(); 00280 } else { 00281 for(int i=1;i<=4;i++) 00282 LEDs.set(i); 00283 } 00284 wait(0.05); 00285 } 00286 }
Generated on Tue Jul 12 2022 20:54:01 by 1.7.2