A quadcopter control Software (Still in development). achieved single axis stability!!!!! released for others benefit. if you'd like to help co-develop this code, then please let me know
Dependencies: MovingAverageFilter MyI2C PID RC mbed-rtos mbed
Currently on hold, due to the fact that i don't own a RX/TX system
main.cpp
- Committer:
- KarimAzzouz
- Date:
- 2013-08-27
- Revision:
- 1:e08a4f517989
- Parent:
- 0:54b67cd15a5b
File content as of revision 1:e08a4f517989:
/* F +X | | | | L ------|------R -Y------|------+Y | | | | B -X */ #include "mbed.h" #include "rtos.h" #include "GlobalDefines.h" #include "MPU6050.h" #include "BMP085.h" //#include "DCM.h" Drifts a lot, needs tuning #include "PID.h" #include "Motors.h" #define Debugging typedef enum{Armed=0, UnArmed }State; State MotorState = Armed; bool print = false; bool P_readingReady=false; bool T_readingReady=false; char c ; uint8 count=0; int16 Temperature=0; int32 Pressure=0; PIDClass PitchPID(4,5,2.5,0.1); // This is a nested PID loop // Kp,Ki,Kp,Kd,iLim..... best till now 4,0,2.5,0.1 MPU6050 Sensor; BMP085 BMP; //DCM IMU; void FlightControl(void const *args){ //Get Sensor Data Sensor.GyroRate(); Sensor.AccAngle(); ///////////////////////BMP085 Contorol switch statement(Work in Progress!!!)////////////////////////////////////// switch(count){ case 0: BMP.readUT_Flag(); count+=1; break; case 1: Temperature=BMP.read_Temperature(); T_readingReady=true; BMP.readUP_Flag(); count+=1; break; case 7: Pressure = BMP.read_Pressure(); P_readingReady=true; count=0; break; default: count+=1; break; } /////////////////////////////////////////////////////////////////////////////////////////// //Gotta Keep Time deltaTime = Global.read(); Global.reset(); pitch = (0.99)*(pitch+Sensor.Scaled_GyroX*deltaTime) + (0.01)*(Sensor.accangle[0]); roll = (0.99)*(roll+ Sensor.Scaled_GyroY*deltaTime) + (0.01)*(Sensor.accangle[1]); //IMU.Update_DCM(deltaTime,ToRad(Sensor.Scaled_GyroX),ToRad(Sensor.Scaled_GyroY),ToRad(Sensor.Scaled_GyroZ),Sensor.Scaled_AccX,Sensor.Scaled_AccY,Sensor.Scaled_AccZ); //Update PID if(Throttle>=1200){ PitchCorrection= PitchPID.update(PitchSetpoint,pitch,Sensor.Scaled_GyroX,deltaTime); } //Motor Commands if(MotorState==Armed){ updateMotors(PitchCorrection,0); } print = true; } int main(){ #ifdef Debugging pc.baud(115200); #endif //Start-Up functions __disable_irq(); CalibrateESC(); // Throttle Sweep on startup two=1; // check sensor connection while(!Sensor.CheckConnection()); //check sensor connection two=0; // sensor okay BMP.Calibrate(); // Calibrations sequence for BMP... TODO: store values in LocalFileSystem Sensor.MPU_Setup(); // Setup MPU6050 Sensor.CalibrateGyro(); // Calibrate Gyros (averaging 100 readings) Sensor.CalibrateAcc(); // Calibrate ACC (averaging 100 readings) one=1; // System okay to start __enable_irq(); RtosTimer Fly(FlightControl,osTimerPeriodic,NULL); // Initialize RTOS Timer Global.start(); // Start timer Fly.start(5); // RTOS Timer (200Hz Control Loop) while(true){ if(pc.readable()){ c = pc.getc(); if(c == 'a'||c=='A') MotorState = Armed; else if(c == 's'||c=='S') MotorState = UnArmed; else if(c == 'u'||c=='U') Throttle+=50 ; else if(c == 'i'||c=='I') Throttle-=50; else if(c=='m'||c=='M') PitchSetpoint+=10; else if(c=='n'||c=='N') PitchSetpoint-=10; //print=true; } #ifdef Debugging if(print){ //pc.printf("%d\n",Temperature); pc.printf("%6.2f\n",pitch); //T_readingReady=false; print=false; } #endif Thread::wait(20); } }