Diff: main.cpp

Revision:

31:872d8b8c7812

Parent:

30:021e13b62575

Child:

32:e2e02338805e

diff -r 021e13b62575 -r 872d8b8c7812 main.cpp
--- a/main.cpp	Sun Feb 10 22:08:10 2013 +0000
+++ b/main.cpp	Sat Mar 30 09:17:44 2013 +0000
@@ -14,11 +14,12 @@
 #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 RC_SENSITIVITY  20
 #define YAWSPEED        2                                   // maximal speed of yaw rotation in degree per Rate
 
-#define P               0.35                                   // PID values
-#define I               0
-#define D               0.5
+float P = 1.5;                                   // PID values
+float I = 0;
+float D = 1;
 
 //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
 #define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC
@@ -53,8 +54,9 @@
 unsigned long   dt_read_sensors = 0;
 unsigned long   time_read_sensors = 0;
 float           tempangle = 0; // temporärer winkel für yaw mit kompass
-float           controller_value[] = {0,0,0};
+float           controller_value[] = {0,0,0};   // The calculated answer form the Controller
 float           RC_angle[] = {0,0,0};  // Angle of the RC Sticks, to steer the QC
+char            command[300]; //= {'\0'};
 
 void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds
 {
@@ -92,8 +94,9 @@
         #endif
     }
     
-    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<2;i++)    // calculate new angle we want the QC to have
+        RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
+    //RC_angle[2] += (RC[3].read()-500)*YAWSPEED/500;
     
     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
@@ -138,9 +141,37 @@
     }
 }
 
+void execute() {
+    if (command[0] == 'p')
+        P = atoi(&command[1]);
+    if (command[0] == 'i')
+        I = atoi(&command[1]);
+    if (command[0] == 'd')
+        D = atoi(&command[1]);
+}
+
+void pc_worker() {
+    char input = pc.getc();
+    
+    if (input == '\r') {
+        execute();
+        command[0] = '\0';
+    } else {
+        int i = 0;
+        while(command[i] != '\0'){
+            i++;
+            LEDs.rollnext();
+            }
+        command[i]   = input;
+        command[i+1] = '\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)
     
+    //pc.attach(&pc_worker); // zum Befehle geben
+    
     #ifdef LOGGER
         Logger = fopen("/local/log.csv", "w"); // Prepare Logfile
         for(int i = 0; i < 3; i++) {
@@ -181,6 +212,8 @@
                 pc.printf("DIS_ARMED            ");
             pc.locate(5,3);
             pc.printf("Roll:%6.1f   Pitch:%6.1f   Yaw:%6.1f    ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
+            pc.locate(5,4);
+            pc.printf("P:%6.1f   I:%6.1f   D:%6.1f    ", P, I, D);
             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);
@@ -200,6 +233,9 @@
             pc.printf("RC1: %4d   ", RC[1].read());
             pc.printf("RC2: %4d   ", RC[2].read());
             pc.printf("RC3: %4d   ", RC[3].read());
+            
+            pc.locate(10,21);
+            pc.printf("Commandline: %s ", command);
         #endif
         if(armed){
             LEDs.rollnext();