Diff: main.cpp

Revision:

30:4820042e67b5

Parent:

29:ae765492fa8b

Child:

31:d473eacfc271

--- a/main.cpp	Fri Apr 15 19:33:14 2016 +0000
+++ b/main.cpp	Wed Apr 20 02:29:37 2016 +0000
@@ -18,12 +18,49 @@
 // to read the battery voltage
 AnalogIn battery(p20);
 DigitalOut batteryLed(LED1);
+DigitalOut shutDownLed(LED2);
 
 int emergencyOff = 0;
+//int lowThrust= {0,0,0};
+int nLowThrust = 0;
+
+void emergencyShutdown()
+{
+    emergencyOff = 1;
+    motor_1 = 0.1;
+    motor_2 = 0.1;
+    motor_3 = 0.1;
+    motor_4 = 0.1;
+}
+
+int getLowThrust(double threshold)
+{
+    double force = myQuadcopter.getForce();
+    if (force < threshold) { // if low thrust signal is detected
+        nLowThrust++;
+        printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust);
+        if (nLowThrust > 5) {
+            return 1;
+        }
+    } else {
+        nLowThrust = 0;
+    }
+    return 0;
+}
 
 void controller_thread(void const *args)
 {
     while(emergencyOff != 1) {
+       // printf(" thrust: %f\r\n",myQuadcopter.getForce());
+
+        int shutdown=  getLowThrust(-0.3);
+        if (shutdown==1) {
+            emergencyShutdown();
+            printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
+            shutDownLed = 1;
+            break;
+        }
+
         myQuadcopter.readSensorValues();
 
         myQuadcopter.controller();
@@ -60,11 +97,8 @@
             printf("low battery! %f\r\n", battery.read() * saturating_voltage);
             batteryLed = 1;
             if (battery.read() < emergency_adc) {
-                emergencyOff = 1;
-                motor_1 = 0.1;
-                motor_2 = 0.1;
-                motor_3 = 0.1;
-                motor_4 = 0.1;
+                emergencyShutdown();
+                break;
             }
         }
         Thread::wait(1000);  // wait for some number of miliseconds
@@ -82,21 +116,21 @@
     motor_4.period(0.01);
 
     Thread threadR(rc_thread);
-    double forceThreshold = -0.3;
-    double forceRc = myQuadcopter.getForce();
+
+
+    int startLoop= 0;
 
-    // TODO figure out why forceRc occasionally goes below the threshold without input
-    while (forceRc > forceThreshold) { // wait until Joystick is in starting position
-        forceRc = myQuadcopter.getForce();
-        wait(1);
+    while (!startLoop) { // wait until Joystick is in starting position
+        startLoop=  getLowThrust(-0.3);
     }
+    nLowThrust = 0; // reset for later use in controller.
 
     motor_1 = 0.1;
     motor_2 = 0.1;
     motor_3 = 0.1;
     motor_4 = 0.1;
 
-    wait(2); // hold startup duty cycle for 2 seconds
+    wait(3); // hold startup duty cycle for 2 seconds
 
     // TODO assign priorities to threads, test if it really works as we expect
     Thread battery(battery_thread);