![](/media/cache/profiles/8ad6635f33710af6a535e8fb8486975e.50x50_q85.jpg)
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
- Committer:
- maetugr
- Date:
- 2012-10-20
- Revision:
- 12:67a06c9b69d5
- Parent:
- 11:9bf69bc6df45
- Child:
- 13:4737ee9ebfee
File content as of revision 12:67a06c9b69d5:
#include "mbed.h" // Standard Library #include "LED.h" // LEDs framework for blinking ;) #include "PC.h" // Serial Port via USB for debugging in TeraTerm (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe) #include "L3G4200D.h" // Gyro (Gyroscope) #include "ADXL345.h" // Acc (Accelerometer) #include "HMC5883.h" // Comp (Compass) #include "BMP085.h" // Alt (Altitude sensor) #include "RC_Channel.h" // RemoteControl Chnnels with PPM #include "Servo.h" // Motor PPM #include "PID.h" // PID Library from Aaron Berk #define PI 3.1415926535897932384626433832795 #define Rad2Deg 57.295779513082320876798154814105 #define RATE 0.02 // speed of Ticker/PID //#define COMPASSCALIBRATE Timer GlobalTimer; // global time to calculate processing speed Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle // initialisation of hardware LED LEDs; PC pc(USBTX, USBRX, 57600); L3G4200D Gyro(p28, p27); ADXL345 Acc(p28, p27); HMC5883 Comp(p28, p27); BMP085 Alt(p28, p27); RC_Channel RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!! Servo Motor[] = {(p15), (p16), (p17), (p18)}; // variables for loop unsigned long dt_get_data = 0; unsigned long time_get_data = 0; unsigned long dt_read_sensors = 0; unsigned long time_read_sensors = 0; float angle[3] = {0,0,0}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw float tempangle = 0; int Motorvalue[3]; float pidtester; void get_Data() { time_read_sensors = GlobalTimer.read_us(); // read data from sensors Gyro.read(); Acc.read(); Comp.read(); //Alt.Update(); dt_read_sensors = GlobalTimer.read_us() - time_read_sensors; // meassure dt dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop time_get_data = GlobalTimer.read_us(); // set new time for next measurement // calculate angles for roll, pitch an yaw angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0; angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0; angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here // TODO Read RC data // calculate new motorspeeds /* Motor[0] = 1000 + (100 + (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000; Motor[1] = 1000 + (100 + (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000; Motor[2] = 1000 + (100 - (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000; Motor[3] = 1000 + (100 - (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000; */ } int main() { //NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts #ifdef COMPASSCALIBRATE pc.locate(10,5); pc.printf("CALIBRATING"); Comp.calibrate(60); #endif // init screen pc.locate(10,5); pc.printf("Flybed v0.2"); LEDs.roll(2); // Start! GlobalTimer.start(); Datagetter.attach(&get_Data, RATE); // start to get data all RATEms while(1) { pc.locate(10,5); // PC output pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); pc.locate(10,8); pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); pc.locate(10,9); pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]); pc.locate(10,10); pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle); pc.locate(10,12); pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle()); pc.locate(10,13); pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); pc.locate(10,15); pc.printf("pidtester: %6.1f RC: %d %d %d %d ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read()); pc.locate(10,19); pc.printf("RC0: %d :[", RC[0].read()); for (int i = 0; i < (RC[0].read() - 1000)/17; i++) pc.printf("="); pc.printf(" "); pc.locate(10,20); pc.printf("RC1: %d :[", RC[1].read()); for (int i = 0; i < (RC[1].read() - 1000)/17; i++) pc.printf("="); pc.printf(" "); pc.locate(10,21); pc.printf("RC2: %d :[", RC[2].read()); for (int i = 0; i < (RC[2].read() - 1000)/17; i++) pc.printf("="); pc.printf(" "); pc.locate(10,22); pc.printf("RC3: %d :[", RC[3].read()); for (int i = 0; i < (RC[3].read() - 1000)/17; i++) pc.printf("="); pc.printf(" "); LEDs.rollnext(); } }