Revision 26:96a072233d7a, committed 2012-11-27

Comitter:

maetugr

Date:

Tue Nov 27 19:49:38 2012 +0000

Parent:

25:0498d3041afa

Child:

27:9e546fa47c33

Commit message:

IMU_Filter and Mixer now new appart from the main

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/IMU_Filter.cpp	Tue Nov 27 19:49:38 2012 +0000
@@ -0,0 +1,55 @@
+#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)
+{
+    get_Acc_angle(Acc_data);
+    for(int i = 0; i < 3; i++)
+        d_Gyro_angle[i] = Gyro_data[i] *dt/15000000.0;
+    
+    // calculate angles for roll, pitch an yaw
+    #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 1 // neuer Test 1 (Formel von http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
+        angle[0] = (0.98*(angle[0]+(Gyro_data[0] *dt/15000000.0)))+(0.02*(Acc_angle[0]));
+        angle[1] = (0.98*(angle[1]+(Gyro_data[1] *dt/15000000.0)))+(0.02*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
+        //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
+        angle[2] += d_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 // rein Gyro
+        for(int i = 0; i < 3; i++)
+            angle[i] = 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/IMU_Filter.h	Tue Nov 27 19:49:38 2012 +0000
@@ -0,0 +1,23 @@
+// 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/Mixer/Mixer.cpp	Tue Nov 27 19:49:38 2012 +0000
@@ -0,0 +1,31 @@
+#include "Mixer.h"
+
+Mixer::Mixer()
+{
+    for(int i=0; i<4; i++)
+        Motor_speed[i]=0;
+}
+
+void Mixer::compute(unsigned long dt, const float * angle, int Throttle, const float * controller_value)
+{
+    // Calculate new motorspeeds
+    // Pitch
+    Motor_speed[0] = Throttle +controller_value[1];
+    Motor_speed[2] = Throttle -controller_value[1];
+    
+    #if 1
+        // Roll
+        Motor_speed[1] = Throttle +controller_value[0];
+        Motor_speed[3] = Throttle -controller_value[0];
+        
+        /*// Yaw
+        Motor_speed[0] -= controller_value[2];
+        Motor_speed[2] -= controller_value[2];
+        
+        Motor_speed[1] += controller_value[2];
+        Motor_speed[3] += controller_value[2];*/
+    #endif
+    
+    for(int i = 0; i < 4; i++) // make shure no motor stands still
+        Motor_speed[i] = Motor_speed[i] > 50 ? Motor_speed[i] : 50;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mixer/Mixer.h	Tue Nov 27 19:49:38 2012 +0000
@@ -0,0 +1,18 @@
+// by MaEtUgR
+
+#ifndef MIXER_H
+#define MIXER_H
+
+#include "mbed.h"
+
+class Mixer
+{
+    public:
+        Mixer();
+        void compute(unsigned long dt, const float * angle, int Throttle, const float * controller_value);
+        float Motor_speed[4];
+    private:
+        
+};
+
+#endif
\ No newline at end of file

--- a/Sensors/Acc/ADXL345.cpp	Mon Nov 26 16:11:28 2012 +0000
+++ b/Sensors/Acc/ADXL345.cpp	Tue Nov 27 19:49:38 2012 +0000
@@ -49,18 +49,6 @@
     data[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
     data[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
     data[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
-    
-    // calculate the angles for roll and pitch (0,1)
-    float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));
-    float temp[3];
-    
-    temp[0] = -(Rad2Deg * acos((float)data[1] / R)-90);
-    temp[1] =   Rad2Deg * acos((float)data[0] / R)-90;
-    temp[2] =   Rad2Deg * acos((float)data[2] / R);
-    
-    for(int i = 0;i < 3; i++)
-        if (temp[i] > -360 && temp[i] < 360)
-            angle[i] = temp[i];
 }
 
 void ADXL345::writeReg(char address, char data){ 

--- a/Sensors/Acc/ADXL345.h	Mon Nov 26 16:11:28 2012 +0000
+++ b/Sensors/Acc/ADXL345.h	Tue Nov 27 19:49:38 2012 +0000
@@ -64,8 +64,6 @@
 #define ADXL345_Y           0x01
 #define ADXL345_Z           0x02
 
-#define Rad2Deg         57.295779513082320876798154814105
-
 typedef char byte;
 
 class ADXL345
@@ -74,7 +72,6 @@
         ADXL345(PinName sda, PinName scl); // constructor, uses i2c
         void read(); // read all axis to array
         int data[3]; // where the measured data is saved
-        float angle[3]; // where the calculated angles are stored
        
     private:
         I2C i2c; // i2c object to communicate

--- a/Servo_PWM/Servo_PWM.h	Mon Nov 26 16:11:28 2012 +0000
+++ b/Servo_PWM/Servo_PWM.h	Tue Nov 27 19:49:38 2012 +0000
@@ -5,35 +5,13 @@
 
 #include "mbed.h"
 
-/** Class to control a servo on any pin, without using pwm
- *
- * Example:
- * @code
- * // Keep sweeping servo from left to right
- * #include "mbed.h"
- * #include "Servo.h"
- * 
- * Servo Servo1(p20);
- *
- * Servo1.Enable(1500,20000);
- *
- * while(1) {
- *     for (int pos = 1000; pos < 2000; pos += 25) {
- *         Servo1.SetPosition(pos);  
- *         wait_ms(20);
- *     }
- *     for (int pos = 2000; pos > 1000; pos -= 25) {
- *         Servo1.SetPosition(pos); 
- *         wait_ms(20); 
- *     }
- * }
- * @endcode
+/** Class to control a servo by using PWM
  */
 
 class Servo_PWM {
 
 public:
-    /** Create a new Servo object on any mbed pin
+    /** Create a new Servo object on any PWM pin
      *
      * @param Pin Pin on mbed to connect servo to
      */
@@ -41,14 +19,18 @@
     
     /** Change the position of the servo. Position in us
      *
-     * @param NewPos The new value of the servos position (us)
+     * @param position The new value of the servos position between 0 and 1000 (gets 1000-2000) (us)
      */
     void SetPosition(int position);
     
-    //operator for confortable positioning
+    /** Operator for confortable positioning
+     *
+     * @param position see SetPosition
+     */
     void operator=(int position);
     
-    // initialize ESC
+    /** initialize ESC
+     */
     void initialize();
 
 private:

--- a/main.cpp	Mon Nov 26 16:11:28 2012 +0000
+++ b/main.cpp	Tue Nov 27 19:49:38 2012 +0000
@@ -8,23 +8,15 @@
 #include "RC_Channel.h" // RemoteControl Chnnels 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 RAD2DEG         57.295779513082320876798154814105   // ratio between radians and degree (360/2Pi) //TODO not needed??
 #define RATE            0.02                                // speed of the interrupt for Sensors and PID
 
-
 #define P_VALUE         0.02                                // PID values
 #define I_VALUE         20                                   // Werte die bis jetzt am besten funktioniert haben
 #define D_VALUE         0.004
 
-
-/*
-// agressive Werte
-#define P_VALUE         0.035                                // PID values
-#define I_VALUE         3.5  
-#define D_VALUE         0.04
-*/
-
 //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
 #define PC_CONNECTED // decoment if you want to debug per USB and your PC
 
@@ -42,23 +34,22 @@
 ADXL345     Acc(p28, p27);
 HMC5883     Comp(p28, p27);
 BMP085_old      Alt(p28, p27);
-RC_Channel  RC[] = {p11, p12, p13, p14};    // noooo p19/p20 !
-Servo_PWM   Motor[] = {p21, p22, p23, p24}; // p21 - p26 only !
+RC_Channel  RC[] = {p11, p12, p13, p14};    // no p19/p20 !
+Servo_PWM   ESC[] = {p21, p22, p23, p24}; // p21 - p26 only because PWM!
+IMU_Filter  IMU;    // don't write () after constructor for no arguments!
+Mixer       MIX;      
 
 // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
 PID     Controller[] = {PID(P_VALUE, I_VALUE, D_VALUE, RATE), PID(P_VALUE, I_VALUE, D_VALUE, RATE), PID(0.02, 100, 0.005, RATE)}; // TODO: RATE != dt immer anpassen
 
 // global variables
 bool            armed = false;          // this variable is for security
-unsigned long   dt_get_data = 0; // TODO: dt namen
-unsigned long   time_get_data = 0;
+unsigned long   dt = 0;
+unsigned long   time_for_dt = 0;
 unsigned long   dt_read_sensors = 0;
 unsigned long   time_read_sensors = 0;
-float           angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
-float           tempangle = 0; // temporärer winkel für yaw ohne kompass
-float           Gyro_angle[3] ={0,0,0};
+float           tempangle = 0; // temporärer winkel für yaw mit kompass
 float           controller_value[] = {0,0,0};
-float           motor_value[] = {0,0,0,0};
 
 void get_Data() // method which is called by the Ticker Datagetter every RATE seconds
 {
@@ -73,37 +64,13 @@
     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
-    
-    for(int i = 0; i < 3; i++)
-        Gyro_angle[i] += Gyro.data[i] *dt_get_data/15000000.0;
-    
-    // calculate angles for roll, pitch an yaw
-    #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
-        angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0;
-        angle[1] += (Acc.angle[1] - 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_angle[2]; // gyro only here
-    #endif
+    dt = GlobalTimer.read_us() - time_for_dt; // time in us since last loop
+    time_for_dt = GlobalTimer.read_us();      // set new time for next measurement
     
-    #if 1 // neuer Test 1 (Formel von http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
-        angle[0] = (0.98*(angle[0]+(Gyro.data[0] *dt_get_data/15000000.0)))+(0.02*(Acc.angle[0]));
-        angle[1] = (0.98*(angle[1]+(Gyro.data[1] *dt_get_data/15000000.0)))+(0.02*(Acc.angle[1] + 3)); // TODO Offset accelerometer einstellen
-        //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
-        angle[2] = Gyro_angle[2]; // gyro only here
-    #endif
+    // TODO: RC_signal füllen!!!
     
-    #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 // rein Gyro
-        for(int i = 0; i < 3; i++)
-            angle[i] = Gyro_angle[i];
-    #endif
+    IMU.compute(dt, Gyro.data, Acc.data);
+    MIX.compute(dt, IMU.angle, RC[2].read(), controller_value);
     
     // Arming / disarming
     if(RC[2].read() < 20 && RC[3].read() > 850) {
@@ -132,30 +99,12 @@
                 //Controller[2].setSetPoint(-(int)((RC[3].read()-424)/424.0*180.0)); // TODO: muss später += werden
             }
             for(int i=0;i<3;i++) {
-                Controller[i].setProcessValue(angle[i]);
+                Controller[i].setProcessValue(IMU.angle[i]);
                 controller_value[i] = Controller[i].compute() - 1000;
             }
-            
-            // Calculate new motorspeeds
-            // Pitch
-            motor_value[0] = RC[2].read() +controller_value[1];
-            motor_value[2] = RC[2].read() -controller_value[1];
-            
-            #if 1
-                // Roll
-                motor_value[1] = RC[2].read() +controller_value[0];
-                motor_value[3] = RC[2].read() -controller_value[0];
-                
-                /*// Yaw
-                motor_value[0] -= controller_value[2];
-                motor_value[2] -= controller_value[2];
-                
-                motor_value[1] += controller_value[2];
-                motor_value[3] += controller_value[2];*/
-            #endif
-            
-            for(int i = 0; i < 4; i++) // make shure no motor stands still
-                motor_value[i] = motor_value[i] > 50 ? motor_value[i] : 50;
+            // Set new motorspeeds
+            for(int i=0;i<4;i++)
+                ESC[i] = (int)MIX.Motor_speed[i];
             
             #ifdef LOGGER
                 // Writing Log
@@ -166,18 +115,15 @@
                 fprintf(Logger, "\r\n");
             #endif
     } else {
+        for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
+                ESC[i] = 0;
         for(int i=0;i<3;i++)
-            Controller[i].reset();
-        for(int i=0;i<4;i++)
-            motor_value[i] = 0;
+            Controller[i].reset(); // TODO: schon ok so? anfangspeek?!
     }
-    // Set new motorspeeds
-    for(int i=0;i<4;i++)
-        Motor[i] = (int)motor_value[i];
 }
 
-int main() { // main programm only used for initialisation and debug output
-    NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)
+int main() { // main programm for initialisation and debug output
+    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)
     
     #ifdef LOGGER
         Logger = fopen("/local/log.csv", "w"); // Prepare Logfile
@@ -198,7 +144,7 @@
         Controller[i].setMode(MANUAL_MODE);//AUTO_MODE);
         Controller[i].setSetPoint(0);
     }
-    Controller[2].setInputLimits(-180.0, 180.0); // yaw 360 grad
+    //Controller[2].setInputLimits(-180.0, 180.0); // yaw 360 grad TODO: Yawsteuerung mit -180 bis 180 grad
     
     #ifdef PC_CONNECTED
         #ifdef COMPASSCALIBRATE
@@ -220,38 +166,28 @@
     while(1) { 
         #ifdef PC_CONNECTED
             pc.locate(30,0); // PC output
-            pc.printf("dt:%dms   dt_sensors:%dus    Altitude:%6.1fm   ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
+            pc.printf("dt:%dms   dt_sensors:%dus    Altitude:%6.1fm   ", dt/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
             pc.locate(5,1);
             if(armed)
                 pc.printf("ARMED!!!!!!!!!!!!!");
             else
                 pc.printf("DIS_ARMED            ");
             pc.locate(5,3);
-            pc.printf("Roll:%6.1f   Pitch:%6.1f   Yaw:%6.1f    ", angle[0], angle[1], angle[2]);
-            pc.printf("\n\r   control Roll: %d   control Pitch: %d           ", (int)((RC[0].read()-440)/440.0*90.0), (int)((RC[1].read()-430)/430.0*90.0));
+            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,6);
-            pc.printf("Gyro_angle: X:%6.1f  Y:%6.1f  Z:%6.1f", Gyro_angle[0], Gyro_angle[1], Gyro_angle[2]);
             
             pc.locate(5,8);
             pc.printf("Acc.data: X:%6d  Y:%6d  Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]); 
-            pc.locate(5,9);
-            pc.printf("Acc.angle: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", Acc.angle[0], Acc.angle[1], Acc.angle[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,12);
-            pc.printf("Motor Result:");
-            for(int i=0;i<4;i++)
-                pc.printf("  %d: %6.1f", i, motor_value[i]);
-            
             pc.locate(5,14);
-            pc.printf(" roll: %d     pitch: %d    ", -(int)((RC[0].read()-440)/440.0*90.0), -(int)((RC[1].read()-430)/430.0*90.0));
+            pc.printf("RC: roll: %d     pitch: %d    ", -(int)((RC[0].read()-440)/440.0*90.0), -(int)((RC[1].read()-430)/430.0*90.0));
             
             pc.locate(10,15);
             pc.printf("Debug_Yaw:  Comp:%6.1f tempangle:%6.1f  ", Comp.get_angle(), tempangle);
@@ -287,10 +223,8 @@
         if(armed){
             LEDs.rollnext();
         } else {
-            LEDs.set(1);
-            LEDs.set(2);
-            LEDs.set(3);
-            LEDs.set(4);
+            for(int i=1;i<=4;i++)
+                LEDs.set(i);
         }
     }
 }