Revision 5:8ea99e98de73, committed 2015-09-11

Comitter:

maetugr

Date:

Fri Sep 11 08:43:35 2015 +0000

Parent:

4:b2efa7f03701

Child:

6:f258093beed9

Commit message:

level mode with 12" props pretty stable indoors; some very small wobles remaining

Changed in this revision

--- a/IMU/MPU9250/MPU9250.cpp	Thu Sep 10 22:02:51 2015 +0000
+++ b/IMU/MPU9250/MPU9250.cpp	Fri Sep 11 08:43:35 2015 +0000
@@ -17,7 +17,7 @@
     5             |10           |13.8     |10           |13.4     |1 
     6             |5            |19.0     |5            |18.6     |1 
     */
-    writeRegister8(MPU9250_CONFIG, 0x00);
+    writeRegister8(MPU9250_CONFIG, 0x03);
     writeRegister8(MPU9250_GYRO_CONFIG, 0x18);              // scales gyros range to +-2000dps
     writeRegister8(MPU9250_ACCEL_CONFIG, 0x08);             // scales accelerometers range to +-4g
 }

--- a/PID/PID.cpp	Thu Sep 10 22:02:51 2015 +0000
+++ b/PID/PID.cpp	Fri Sep 11 08:43:35 2015 +0000
@@ -6,6 +6,7 @@
     Integral = 0;
     LastTime = 0;
     Integrate = true;
+    RollBufferIndex = 0;
     PID::P = P;
     PID::I = I;
     PID::D = D;
@@ -29,7 +30,11 @@
         Integral += Error * dt;
         
     // Derivative
-    float Derivative = (Error - PreviousError) / dt;
+    RollBuffer[RollBufferIndex] = (Error - PreviousError) / dt;
+    float Derivative = 0;
+    for(int i=0; i<BUFFERSIZE ;i++)
+        Derivative += RollBuffer[i];
+    Derivative /= BUFFERSIZE;
     PreviousError = Error;
     
     // Final Formula

--- a/PID/PID.h	Thu Sep 10 22:02:51 2015 +0000
+++ b/PID/PID.h	Fri Sep 11 08:43:35 2015 +0000
@@ -5,6 +5,8 @@
 
 #include "mbed.h"
 
+#define BUFFERSIZE 5
+
 class PID {
     public:
         PID(float P, float I, float D, float Integral_Max);
@@ -25,6 +27,8 @@
         bool Integrate; // if the integral is used / the controller is in use
         
         float PreviousError; // the Error of the last computation to get derivative
+        float RollBuffer[BUFFERSIZE];  // Rollingbufferarray for derivative to filter noise
+        int RollBufferIndex;
 };
 
 #endif
\ No newline at end of file

--- a/main.cpp	Thu Sep 10 22:02:51 2015 +0000
+++ b/main.cpp	Fri Sep 11 08:43:35 2015 +0000
@@ -15,7 +15,7 @@
 
 #define PPM_FREQU       495     // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
 #define INTEGRAL_MAX    300     // maximal output offset that can result from integrating errors
-#define RC_SENSITIVITY  30      // maximal angle from horizontal that the PID is aming for
+#define RC_SENSITIVITY  15      // maximal angle from horizontal that the PID is aming for
 #define YAWSPEED        1.0     // maximal speed of yaw rotation in degree per Rate
 #define AILERON         0       // RC
 #define ELEVATOR        1
@@ -34,8 +34,8 @@
 bool  debug = true;                     // shows if we want output for the computer
 bool  level = false;                     // switches between self leveling and acro mode
 bool  RC_present = false;               // shows if an RC is present
-float P_R = 3.3, I_R = 0, D_R = 0;      // PID values for the rate controller
-float P_A = 2.6, I_A = 0, D_A = 0;        // PID values for the angle controller      P_A = 1.865, I_A = 1.765, D_A = 0
+float P_R = 3.3, I_R = 1.1, D_R = 0;      // PID values for the rate controller
+float P_A = 2.2, I_A = 0, D_A = 0;        // PID values for the angle controller      P_A = 1.865, I_A = 1.765, D_A = 0
 float PY = 2.3, IY = 0, DY = 0;         // PID values for Yaw
 float RC_angle[] = {0,0,0};             // Angle of the RC Sticks, to steer the QC
 float Motor_speed[4] = {0,0,0,0};       // Mixed Motorspeeds, ready to send
@@ -74,20 +74,25 @@
         P_R -= 0.1;
         
     if (command == 'e')
-        P_A += 0.1;
+        I_R += 0.1;
     if (command == 'd')
-        P_A -= 0.1;
+        I_R -= 0.1;
         
     if (command == 'r')
-        PY += 0.1;
+        P_A += 0.1;
     if (command == 'f')
-        PY -= 0.1;
+        P_A -= 0.1;
         
     if (command == 't')
         I_A += 0.1;
     if (command == 'g')
         I_A -= 0.1;
         
+    if (command == 'z')
+        PY += 0.1;
+    if (command == 'h')
+        PY -= 0.1;
+        
     pc.putc(command);
     LEDs.tilt(2);
 }
@@ -178,7 +183,7 @@
                 //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50;
                 //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50;
                 for(int i=0;i<4;i++)   // Set new motorspeeds
-                    ESC[i] = (int)Motor_speed[i]>50 ? (int)Motor_speed[i] : 50;
+                    ESC[i] = (int)Motor_speed[i]>100 ? (int)Motor_speed[i] : 100;
                 
         } else {
             for(int i=0;i<4;i++) // for security reason, set every motor to zero speed