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.
Diff: main.cpp
- Revision:
- 40:2ca410923691
- Parent:
- 39:9fd3f4439978
diff -r 9fd3f4439978 -r 2ca410923691 main.cpp --- a/main.cpp Wed Aug 28 00:30:38 2013 +0000 +++ b/main.cpp Fri Feb 14 14:17:32 2014 +0000 @@ -1,10 +1,11 @@ #include "mbed.h" // Standard Library #include "LED.h" // LEDs framework for blinking ;) #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) -#include "L3G4200D.h" // Gyro (Gyroscope) -#include "ADXL345.h" // Acc (Accelerometer) -#include "HMC5883.h" // Comp (Compass) -#include "BMP085_old.h" // Alt (Altitude sensor) +#include "MPU6050.h" // Combined Gyro and Acc +//#include "L3G4200D.h" // Gyro (Gyroscope) +//#include "ADXL345.h" // Acc (Accelerometer) +//#include "HMC5883.h" // Comp (Compass) +//#include "BMP085_old.h" // Alt (Altitude sensor) #include "RC_Channel.h" // RemoteControl Channels with PPM #include "Servo_PWM.h" // Motor PPM using PwmOut #include "PID.h" // PID Library (slim, self written) @@ -27,7 +28,7 @@ #define PITCH 1 #define YAW 2 -#define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC +#define PC_CONNECTED // decomment if you want to debug per USB/Bluetooth and your PC // Global variables bool armed = false; // this variable is for security (when false no motor rotates any more) @@ -54,13 +55,14 @@ // Initialisation of hardware (see includes for more info) LED LEDs; #ifdef PC_CONNECTED - //PC pc(USBTX, USBRX, 115200); // USB - PC pc(p9, p10, 115200); // Bluetooth + PC pc(USBTX, USBRX, 115200); // USB + //PC pc(p9, p10, 115200); // Bluetooth #endif -L3G4200D Gyro(p28, p27); -ADXL345 Acc(p28, p27); -HMC5883 Comp(p28, p27); -BMP085_old Alt(p28, p27); +MPU6050 Sensor(p28, p27); +//L3G4200D Gyro(p28, p27); +//ADXL345 Acc(p28, p27); +//HMC5883 Comp(p28, p27); +//BMP085_old Alt(p28, p27); RC_Channel RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)}; // no p19/p20 ! 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! IMU_Filter IMU; // (don't write () after constructor for no arguments!) @@ -71,8 +73,9 @@ time_read_sensors = GlobalTimer.read(); // start time measure for sensors // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes - Gyro.read(); - Acc.read(); + Sensor.read(); + //Gyro.read(); + //Acc.read(); //Comp.read(); // TODO: not every loop every sensor? altitude not that important //Alt.Update(); // TODO needs very long to read because of waits @@ -84,7 +87,8 @@ dt = GlobalTimer.read() - time_for_dt; // time in us since last loop time_for_dt = GlobalTimer.read(); // set new time for next measurement - IMU.compute(dt, Gyro.data, Acc.data); + IMU.compute(dt, Sensor.data_gyro, Sensor.data_acc); + //IMU.compute(dt, Gyro.data, Acc.data); //pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors); if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100) @@ -147,7 +151,7 @@ for(int i=0;i<4;i++) // for security reason, set every motor to zero speed ESC[i] = 0; } - pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", + /*pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", armed, dt, dt_read_sensors, @@ -163,6 +167,24 @@ MIX.Motor_speed[0], MIX.Motor_speed[1], MIX.Motor_speed[2], + MIX.Motor_speed[3]);*/ + + pc.printf("%f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n", + IMU.angle[ROLL], + IMU.angle[PITCH], + IMU.angle[YAW], + Sensor.data_gyro[0], + Sensor.data_gyro[1], + Sensor.data_gyro[2], + Sensor.data_acc[0], + Sensor.data_acc[1], + Sensor.data_acc[2], + controller_value[ROLL], + controller_value[PITCH], + controller_value[YAW], + MIX.Motor_speed[0], + MIX.Motor_speed[1], + MIX.Motor_speed[2], MIX.Motor_speed[3]); dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time for loop @@ -200,8 +222,9 @@ #endif LEDs.roll(2); - Gyro.calibrate(50, 0.02); - Acc.calibrate(50, 0.02); + Sensor.calibrate(50, 0.02); + //Gyro.calibrate(50, 0.02); + //Acc.calibrate(50, 0.02); // Start! GlobalTimer.start();