Revision 30:021e13b62575, committed 2013-02-10

Comitter:

maetugr

Date:

Sun Feb 10 22:08:10 2013 +0000

Parent:

29:8b7362a2ee14

Child:

31:872d8b8c7812

Commit message:

newest changes because of the needed faster refresh rate for the ESCs 495Hz and the new build of the same hardware in X-configuration.; ; RC angle steering not yet finished

Changed in this revision

--- a/IMU/IMU_Filter.cpp	Sat Dec 15 08:42:36 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,55 +0,0 @@
-#include "IMU_Filter.h"
-
-IMU_Filter::IMU_Filter()
-{
-    for(int i=0; i<3; i++)
-        angle[i]=0;
-}
-
-void IMU_Filter::compute(unsigned long dt, const float * Gyro_data, const int * Acc_data)
-{
-    // calculate angles for each sensor
-    for(int i = 0; i < 3; i++)
-        d_Gyro_angle[i] = Gyro_data[i] *dt/15000000.0;
-    get_Acc_angle(Acc_data);
-    
-    // Complementary Filter
-    #if 1 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
-        angle[0] = (0.99*(angle[0] + d_Gyro_angle[0]))+(0.01*(Acc_angle[0]));
-        angle[1] = (0.99*(angle[1] + d_Gyro_angle[1]))+(0.01*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
-        angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
-    #endif
-    
-    #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
-        angle[0] += (Acc.angle[0] - angle[0])/50 + d_Gyro_angle[0];
-        angle[1] += (Acc.angle[1] - angle[1])/50 + d_Gyro_angle[1];// TODO Offset accelerometer einstellen
-        //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
-        angle[2] = Gyro_angle[2]; // gyro only here
-    #endif
-    
-    #if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
-        angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
-        angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
-        angle[2] = Gyro_angle[2]; // gyro only here
-    #endif
-    
-    #if 0 // all gyro only
-        for(int i = 0; i < 3; i++)
-            angle[i] += d_Gyro_angle[i];
-    #endif
-}
-
-void IMU_Filter::get_Acc_angle(const int * Acc_data)
-{
-    // calculate the angles for roll and pitch (0,1)
-    float R = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
-    float temp[3];
-    
-    temp[0] = -(Rad2Deg * acos((float)Acc_data[1] / R)-90);
-    temp[1] =   Rad2Deg * acos((float)Acc_data[0] / R)-90;
-    temp[2] =   Rad2Deg * acos((float)Acc_data[2] / R);
-    
-    for(int i = 0;i < 3; i++)
-        if (temp[i] > -360 && temp[i] < 360)
-            Acc_angle[i] = temp[i];
-}
\ No newline at end of file

--- a/IMU/IMU_Filter.h	Sat Dec 15 08:42:36 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-// by MaEtUgR
-
-#ifndef IMU_FILTER_H
-#define IMU_FILTER_H
-
-#include "mbed.h"
-
-#define Rad2Deg         57.295779513082320876798154814105 // factor between radians and degrees of angle (180/Pi)
-
-class IMU_Filter
-{
-    public:
-        IMU_Filter();
-        void compute(unsigned long dt, const float * gyro_data, const int * acc_data);
-        float angle[3];                                 // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
-    private:
-        float d_Gyro_angle[3];
-        void get_Acc_angle(const int * Acc_data);
-        float Acc_angle[3];
-};
-
-#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_Filter/IMU_Filter.cpp	Sun Feb 10 22:08:10 2013 +0000
@@ -0,0 +1,57 @@
+#include "IMU_Filter.h"
+
+IMU_Filter::IMU_Filter()
+{
+    for(int i=0; i<3; i++)
+        angle[i]=0;
+}
+
+void IMU_Filter::compute(unsigned long dt, const float * Gyro_data, const int * Acc_data)
+{
+    // calculate angles for each sensor
+    for(int i = 0; i < 3; i++)
+        d_Gyro_angle[i] = Gyro_data[i] *dt/15000000.0;
+    get_Acc_angle(Acc_data);
+    
+    // Complementary Filter
+    #if 1 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
+        angle[0] = (0.999*(angle[0] + d_Gyro_angle[0]))+(0.001*(Acc_angle[0]));
+        angle[1] = (0.999*(angle[1] + d_Gyro_angle[1]))+(0.001*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
+        angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
+    #endif
+    
+    #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
+        angle[0] += (Acc.angle[0] - angle[0])/50 + d_Gyro_angle[0];
+        angle[1] += (Acc.angle[1] - angle[1])/50 + d_Gyro_angle[1];// TODO Offset accelerometer einstellen
+        //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
+        angle[2] = Gyro_angle[2]; // gyro only here
+    #endif
+    
+    #if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
+        angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
+        angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
+        angle[2] = Gyro_angle[2]; // gyro only here
+    #endif
+    
+    #if 0 // all gyro only
+        for(int i = 0; i < 3; i++)
+            angle[i] += d_Gyro_angle[i];
+    #endif
+    
+    
+}
+
+void IMU_Filter::get_Acc_angle(const int * Acc_data)
+{
+    // calculate the angles for roll and pitch (0,1)
+    float R = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
+    float temp[3];
+    
+    temp[0] = -(Rad2Deg * acos((float)Acc_data[1] / R)-90);
+    temp[1] =   Rad2Deg * acos((float)Acc_data[0] / R)-90;
+    temp[2] =   Rad2Deg * acos((float)Acc_data[2] / R);
+    
+    for(int i = 0;i < 3; i++)
+        if (temp[i] > -360 && temp[i] < 360)
+            Acc_angle[i] = temp[i];
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_Filter/IMU_Filter.h	Sun Feb 10 22:08:10 2013 +0000
@@ -0,0 +1,22 @@
+// by MaEtUgR
+
+#ifndef IMU_FILTER_H
+#define IMU_FILTER_H
+
+#include "mbed.h"
+
+#define Rad2Deg         57.295779513082320876798154814105 // factor between radians and degrees of angle (180/Pi)
+
+class IMU_Filter
+{
+    public:
+        IMU_Filter();
+        void compute(unsigned long dt, const float * gyro_data, const int * acc_data);
+        float angle[3];                                 // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
+    private:
+        float d_Gyro_angle[3];
+        void get_Acc_angle(const int * Acc_data);
+        float Acc_angle[3];
+};
+
+#endif
\ No newline at end of file

--- a/Mixer/Mixer.cpp	Sat Dec 15 08:42:36 2012 +0000
+++ b/Mixer/Mixer.cpp	Sun Feb 10 22:08:10 2013 +0000
@@ -1,31 +1,38 @@
 #include "Mixer.h"
 
-Mixer::Mixer()
+Mixer::Mixer(int Configuration)
 {
+    Mixer::Configuration = Configuration;
+    
     for(int i=0; i<4; i++)
         Motor_speed[i]=0;
 }
 
 void Mixer::compute(unsigned long dt, int Throttle, const float * controller_value)
 {
-    // Calculate new motorspeeds
-    
-    for(int i=0; i<4; i++)
-        Motor_speed[i] = Throttle;
-    
-    Motor_speed[1] -= controller_value[0]; // Roll
-    Motor_speed[3] += controller_value[0];
+    // Mixing tables for each configuration
+    float mix_table[2][4][3] = {
+    {
+        {   0,  1,  1},       // + configuration
+        {   1,  0, -1},
+        {   0, -1,  1},
+        {  -1,  0, -1}
+    },
+    {
+        {  RT,  RT,  1},       // X configuration
+        { -RT,  RT, -1},
+        { -RT, -RT,  1},
+        {  RT, -RT, -1}
+    }
+    };
     
-    Motor_speed[0] -= controller_value[1]; // Pitch
-    Motor_speed[2] += controller_value[1];
+    // Calculate new motorspeeds
+    for(int i=0; i<4; i++) {
+        Motor_speed[i] = Throttle;
+        for(int j = 0; j < 3; j++)
+            Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j];
+    }
     
-    #if 0
-        Motor_speed[1] -= controller_value[2]; // Yaw
-        Motor_speed[3] -= controller_value[2];
-        Motor_speed[0] += controller_value[2];
-        Motor_speed[2] += controller_value[2];
-    #endif
-    
-    for(int i = 0; i < 4; i++) // make shure no motor stands still
+    for(int i = 0; i < 4; i++) // make sure no motor stands still
         Motor_speed[i] = Motor_speed[i] > 50 ? Motor_speed[i] : 50;
 }
\ No newline at end of file

--- a/Mixer/Mixer.h	Sat Dec 15 08:42:36 2012 +0000
+++ b/Mixer/Mixer.h	Sun Feb 10 22:08:10 2013 +0000
@@ -3,16 +3,18 @@
 #ifndef MIXER_H
 #define MIXER_H
 
+#define RT          0.70710678118654752440084436210485      // 1 / squrt(2) for the X configuration
+
 #include "mbed.h"
 
 class Mixer
 {
     public:
-        Mixer();
+        Mixer(int Configuration);
         void compute(unsigned long dt, int Throttle, const float * controller_value);
-        float Motor_speed[4];
+        float Motor_speed[4];       // calculated motor speeds to send to the ESCs
     private:
-        
+        int Configuration;          // number of the configuration used (for example +)
 };
 
 #endif
\ No newline at end of file

--- a/PID/PID.h	Sat Dec 15 08:42:36 2012 +0000
+++ b/PID/PID.h	Sun Feb 10 22:08:10 2013 +0000
@@ -1,3 +1,5 @@
+// by MaEtUgR
+
 #include "mbed.h"
 
 #ifndef PID_H

--- a/main.cpp	Sat Dec 15 08:42:36 2012 +0000
+++ b/main.cpp	Sun Feb 10 22:08:10 2013 +0000
@@ -5,14 +5,14 @@
 #include "ADXL345.h"    // Acc (Accelerometer)
 #include "HMC5883.h"    // Comp (Compass)
 #include "BMP085_old.h"     // Alt (Altitude sensor)
-#include "RC_Channel.h" // RemoteControl Chnnels with PPM
+#include "RC_Channel.h" // RemoteControl Channels with PPM
 #include "Servo_PWM.h"  // Motor PPM using PwmOut
 #include "PID.h"        // PID Library by Aaron Berk
 #include "IMU_Filter.h" // Class to calculate position angles
 #include "Mixer.h"      // Class to calculate motorspeeds from Angles, Regulation and RC-Signals
 
-#define RATE            0.02                                // speed of the interrupt for Sensors and PID
-#define PPM_FREQU       490                                 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
+#define RATE            0.002                               // speed of the interrupt for Sensors and PID
+#define PPM_FREQU       495                                 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
 #define MAXPITCH        40                                  // maximal angle from horizontal that the PID is aming for
 #define YAWSPEED        2                                   // maximal speed of yaw rotation in degree per Rate
 
@@ -29,19 +29,19 @@
 // initialisation of hardware (see includes for more info)
 LED         LEDs;
 #ifdef PC_CONNECTED
-    PC          pc(USBTX, USBRX, 38400);    // USB
+    PC          pc(USBTX, USBRX, 115200);    // USB
     //PC          pc(p9, p10, 115200);      // Bluetooth
 #endif
 LocalFileSystem local("local");               // Create the local filesystem under the name "local"
-FILE *Logger;
+//FILE *Logger;
 L3G4200D    Gyro(p28, p27);
 ADXL345     Acc(p28, p27);
 HMC5883     Comp(p28, p27);
 BMP085_old      Alt(p28, p27);
-RC_Channel  RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,3), RC_Channel(p14,4)};    // no p19/p20 !
+RC_Channel  RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,4), RC_Channel(p14,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!
-Mixer       MIX;      
+Mixer       MIX(1);      
 
 // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
 PID     Controller[] = {PID(P, I, D, 1000), PID(P, I, D, 1000), PID(0.2, 0, 0.1, 1000)};
@@ -54,7 +54,7 @@
 unsigned long   time_read_sensors = 0;
 float           tempangle = 0; // temporärer winkel für yaw mit kompass
 float           controller_value[] = {0,0,0};
-float           AnglePosition[] = {0,0,0};
+float           RC_angle[] = {0,0,0};  // Angle of the RC Sticks, to steer the QC
 
 void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds
 {
@@ -77,8 +77,6 @@
     // Arming / disarming
     if(RC[3].read() < 20 && RC[2].read() > 850) {
         armed = true;
-        for(int i=0;i<3;i++)
-                AnglePosition[i] = IMU.angle[i];
         #ifdef LOGGER
             if(Logger == NULL)
                 Logger = fopen("/local/log.csv", "a");
@@ -94,14 +92,20 @@
         #endif
     }
     
-    for(int i=0;i<3;i++)
+    for(int i=0;i<3;i++)    // calculate new angle we want the QC to have
+        RC_angle[i] = (RC[i].read()-500)*30/500.0;
+    
+    for(int i=0;i<3;i++) {
         Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
+        controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct
+    }
+                
     
     if (armed) // for SECURITY!
     {       
             // RC controlling
-            for(int i=0;i<3;i++)
-                AnglePosition[i] -= (RC[i].read()-500)*2/500.0;
+            /*for(int i=0;i<3;i++)
+                AnglePosition[i] -= (RC[i].read()-500)*2/500.0;*/
             /*virt_angle[0] = IMU.angle[0] + (RC[0].read()-500)*MAXPITCH/500.0; // TODO: zuerst RC calibration
             virt_angle[1] = IMU.angle[1] + (RC[1].read()-500)*MAXPITCH/500.0;
             yawposition += (RC[3].read()-500)*YAWSPEED/500;
@@ -113,8 +117,7 @@
                 Controller[1].setSetPoint(-((RC[1].read()-500)*MAXPITCH/500.0));
                 Controller[2].setSetPoint(-((RC[3].read()-500)*180.0/500.0));
             }*/
-            for(int i=0;i<3;i++)
-                controller_value[i] = Controller[i].compute(AnglePosition[i], IMU.angle[i]); // gove the controller the actual angle and get his advice to correct
+            
             
             MIX.compute(dt, RC[3].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
             
@@ -149,8 +152,6 @@
         Logger = NULL;
     #endif
     
-    //Controller[2].setInputLimits(-180.0, 180.0); // yaw 360 grad TODO: Yawsteuerung mit -180 bis 180 grad
-    
     #ifdef PC_CONNECTED
         #ifdef COMPASSCALIBRATE
             pc.locate(10,5);
@@ -168,16 +169,11 @@
     GlobalTimer.start();
     Dutycycler.attach(&dutycycle, RATE);     // start to process all RATEms
     
-    int count = 0;
     while(1) { 
-        //pc.printf("%6.1f,%6.1f,%6.1f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
-        #if 1
-            if(count == 20){
-                //pc.cls();
-                count = 0;
-            }
+        //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]); // serialplot of IMU
+        #if 1 //pc.cls();
             pc.locate(30,0); // PC output
-            pc.printf("dt:%dms   dt_sensors:%dus    Altitude:%6.1fm   ", dt/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
+            pc.printf("dt:%3.3fms   dt_sensors:%dus    Altitude:%6.1fm   ", dt/1000.0, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
             pc.locate(5,1);
             if(armed)
                 pc.printf("ARMED!!!!!!!!!!!!!");
@@ -187,14 +183,14 @@
             pc.printf("Roll:%6.1f   Pitch:%6.1f   Yaw:%6.1f    ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
             pc.locate(5,5);
             pc.printf("Gyro.data: X:%6.1f  Y:%6.1f  Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
-            pc.locate(5,8);
+            pc.locate(5,6);
             pc.printf("Acc.data: X:%6d  Y:%6d  Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]); 
             pc.locate(5,11);
             pc.printf("PID Result:");
             for(int i=0;i<3;i++)
                 pc.printf("  %d: %6.1f", i, controller_value[i]);
             pc.locate(5,14);
-            pc.printf("RC controll: roll: %f     pitch: %f      yaw: %f    ", AnglePosition[0], AnglePosition[1], AnglePosition[2]);
+            pc.printf("RC angle: roll: %f     pitch: %f      yaw: %f    ", RC_angle[0], RC_angle[1], RC_angle[2]);
             pc.locate(5,16);
             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]);
             
@@ -212,6 +208,5 @@
                 LEDs.set(i);
         }
         wait(0.05);
-        count++;
     }
 }
\ No newline at end of file