Diff: main.cpp

Revision:

8:609a2ad4c30e

Parent:

7:90f876d47862

--- a/main.cpp	Mon Sep 14 12:49:08 2015 +0000
+++ b/main.cpp	Thu Nov 19 18:47:27 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  15      // maximal angle from horizontal that the PID is aming for
+#define RC_SENSITIVITY  30      // 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,20 +34,22 @@
 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 = 2.5, I_R = 0.3, D_R = 0;      // PID values for the rate controller
-float P_A = 2.1, I_A = 0.3, D_A = 0;        // PID values for the angle controller      P_A = 1.865, I_A = 1.765, D_A = 0
+float P_R = 2.6, I_R = 0.3, D_R = 0;      // PID values for the rate controller
+float P_A = 1.9, I_A = 0.2, 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
 
 Timer LoopTimer;
 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
-float control_frequency = PPM_FREQU;         // frequency for the main loop in Hz
+float control_frequency = 800;//PPM_FREQU;         // frequency for the main loop in Hz
+int counter = 0;
+int divider = 20;
 
 LED         LEDs;
-PC          pc(USBTX, USBRX, 115200);   // USB
-//PC          pc(p9, p10, 115200);       // Bluetooth PIN: 1234
-IMU_10DOF   IMU(p5, p6, p7, p19);
+//PC          pc(USBTX, USBRX, 115200);   // USB
+PC          pc(p9, p10, 115200);       // Bluetooth PIN: 1234
+IMU_10DOF   IMU(p5, p6, p7, p19, p28, p27);
 RC_Channel  RC[] = {RC_Channel(p8,1), RC_Channel(p15,2), RC_Channel(p17,4), RC_Channel(p16,3), RC_Channel(p25,2), RC_Channel(p26,4), RC_Channel(p29,3)}; // no p19/p20 !
 PID         Controller_Rate[] = {PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
 PID         Controller_Angle[] = {PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(0, 0, 0, INTEGRAL_MAX)};
@@ -103,14 +105,17 @@
     if (level) {
         for(int i=0;i<2;i++) { // LEVEL
             Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
-            Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct
+            if (counter % 16 == 0)
+                Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angles and get his advice to correct
             Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
-            Controller_Rate[i].compute(-Controller_Angle[i].Value, IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            Controller_Rate[i].compute(-Controller_Angle[i].Value, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            //Controller_Rate[i].compute(-Controller_Angle[i].Value, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 );
         }
     } else {
         for(int i=0;i<2;i++) { // ACRO
             Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
-            Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            //Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 );
         }
     }
     
@@ -130,22 +135,22 @@
     Motor_speed[3] = throttle   +SQRT2*Controller_Rate[ROLL].Value  +SQRT2*Controller_Rate[PITCH].Value;  // 
     
     Motor_speed[0] -= Controller_Rate[YAW].Value;
+    Motor_speed[1] += Controller_Rate[YAW].Value;
     Motor_speed[2] -= Controller_Rate[YAW].Value;
     Motor_speed[3] += Controller_Rate[YAW].Value;
-    Motor_speed[1] += Controller_Rate[YAW].Value;
     Times[5] = LoopTimer.read(); // 17us
     
     if (armed) // for SECURITY!
     {       
             debug = false;
             // PITCH
-            ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50;
-            ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50;
+            //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50;
+            //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50;
             // ROLL
             //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]>100 ? (int)Motor_speed[i] : 100;*/
+            for(int i=0;i<4;i++)   // Set new motorspeeds
+                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
@@ -156,9 +161,13 @@
     
     LEDs.rollnext();
     
-    Times[7] = LoopTimer.read(); // 7us TOTAL 297us
+    /*if(counter % divider == 0) {
+        pc.printf("%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
+    }*/
+    counter++;
     
-    while(LoopTimer.read() < 1/control_frequency);
+    Times[7] = LoopTimer.read(); // 7us TOTAL 297us
+    while(LoopTimer.read() < 1/control_frequency); // Kill the rest of the time TODO: make a better solution so we can do misc things with these cycles
     Times[8] = LoopTimer.read();
     LoopTimer.stop();
     LoopTimer.reset();
@@ -167,17 +176,18 @@
     if (debug) {
         pc.printf("$STATE,%d,%d,%.f,%.3f,%.3f\r\n", armed, level, control_frequency, IMU.dt*1e3, IMU.dt_sensors*1e6);
         //pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read());
-        //pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
+        pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
+        pc.printf("$GYRO2,%.3f,%.3f,%.3f\r\n", IMU.mpu2.data_gyro[ROLL], IMU.mpu2.data_gyro[PITCH], IMU.mpu2.data_gyro[YAW]);
         //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]);
         pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]);
         //pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]);
         pc.printf("$CONTR,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Rate[ROLL].Value, Controller_Rate[PITCH].Value, Controller_Rate[YAW].Value, P_R, I_R, D_R, PY);
         pc.printf("$CONTA,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Angle[ROLL].Value, Controller_Angle[PITCH].Value, Controller_Angle[YAW].Value, P_A, I_A, D_A);
         pc.printf("$MOT,%d,%d,%d,%d\r\n", (int)Motor_speed[0], (int)Motor_speed[1], (int)Motor_speed[2], (int)Motor_speed[3]);
-        pc.printf("$TIMES");
+        /*pc.printf("$TIMES");
         for(int i = 1; i < 10; i++)
             pc.printf(",%.3f", (Times[i]-Times[i-1])*1e6);
-        pc.printf("\r\n");
+        pc.printf("\r\n");*/
         wait(0.1);
     }
 }
@@ -209,6 +219,11 @@
     if (command == 'd')
         I_R -= 0.1;
         
+    if (command == 'x')
+        D_R += 0.001;
+    if (command == 'c')
+        D_R -= 0.001;
+        
     if (command == 'r')
         P_A += 0.1;
     if (command == 'f')
@@ -224,12 +239,15 @@
     if (command == 'h')
         PY -= 0.1;
         
-    /*if (command == 'o') {
+    if (command == 'o') {
         control_frequency += 100;
+        
     }
     if (command == 'l') {
         control_frequency -= 100;
-    }*/
+        
+    }
+        
         
     pc.putc(command);
     LEDs.tilt(2);