Diff: main.cpp

Revision:

38:14bf11115f9f

Parent:

37:a983eb9fd9c5

Child:

39:fff0a72633ee

--- a/main.cpp	Sun May 01 18:45:02 2016 +0000
+++ b/main.cpp	Sun May 01 21:59:10 2016 +0000
@@ -24,17 +24,19 @@
 AnalogIn battery(p20);
 DigitalOut batteryLed(LED1);
 DigitalOut shutDownLed(LED2);
+DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement
 
 int emergencyOff = 0;
 //int lowThrust= {0,0,0};
 int nLowThrust = 0;
 
-void print_count(int id) {
+void print_count(int id)
+{
     static int count = 0;
     if (count % 1000 == 0) {
         printf("%d\t%d\r\n", id, count);
     }
-    count++; 
+    count++;
 }
 
 void emergencyShutdown()
@@ -65,7 +67,7 @@
 void controller_thread(void const *args)
 {
     while(emergencyOff != 1) {
-       // printf(" thrust: %f\r\n",myQuadcopter.getForce());
+        // printf(" thrust: %f\r\n",myQuadcopter.getForce());
         myQuadcopter.readSensorValues();
 
         myQuadcopter.controller();
@@ -94,11 +96,11 @@
         if (count % 1000 == 0) {
             printf("%s\t%d\r\n", "rc", count);
         }
-        count++; 
+        count++;
         myQuadcopter.readRc();
 
         if (!started) {
-            continue;   
+            continue;
         }
         int shutdown = getLowThrust(-0.3);
         if (shutdown==1) {
@@ -127,7 +129,7 @@
         if (count % 1 == 0) {
             //printf("%s\t%d\r\n", "battery", count);
         }
-        count++; 
+        count++;
         if (battery.read() < threshold_adc) {
             printf("low battery! %f\r\n", battery.read() * saturating_voltage);
             batteryLed = 1;
@@ -140,14 +142,16 @@
     }
 }
 
-void counting_thread1(void const *args) {
+void counting_thread1(void const *args)
+{
     while (true) {
         print_count(1);
         Thread::wait(1);
     }
 }
 
-void counting_thread2(void const *args) {
+void counting_thread2(void const *args)
+{
     while (true) {
         print_count(2);
     }
@@ -185,16 +189,27 @@
     //Thread battery(battery_thread);
     //Thread controller(controller_thread);
     // TODO is this needed?
-    
+
     //controller.set_priority(osPriorityRealtime);
     //thread_rc.set_priority(osPriorityRealtime);
-    int i =0;
+    int i = 0;
+    float previousTime =0;
+    float currentTime = 0;
     while (1) {
         i++;
-        if (i % 100 == 0) {
-            printf("%d\r\n", i);
-        }
+
+       /* if (i % 5000 == 0) {
+            currentTime = timer.read();
+            printf("%d, %f, %f\r\n", i, 5000.0 / (currentTime - previousTime), currentTime);
+            previousTime = currentTime;
+
+        } */
+        
+        
         myQuadcopter.readRc();
+        
+        
+        
         int shutdown = getLowThrust(-0.3);
         if (shutdown == 1) {
             emergencyShutdown();
@@ -203,6 +218,14 @@
             break;
         }
         myQuadcopter.readSensorValues();
+       /* state mystate = myQuadcopter.getState();
+        if (mystate.phi > 0.0785) {
+            quicknessLed = 1;
+        } else {
+            quicknessLed = 0;
+        } */
+
+
         myQuadcopter.controller();
         motors motorsPwm = myQuadcopter.getPwm();
         motor_1 = motorsPwm.m1;