Diff: main.cpp

Revision:

26:7f50323c0c0d

Parent:

25:d44610851105

Child:

27:11116aa69f32

--- a/main.cpp	Wed Apr 13 19:16:03 2016 +0000
+++ b/main.cpp	Thu Apr 14 19:58:42 2016 +0000
@@ -17,10 +17,15 @@
 
 // to read the battery voltage
 AnalogIn battery(p20);
+DigitalOut batteryLed(LED1);
+
+
+//
+int emergencyOff=0;
 
 void controller_thread(void const *args)
 {
-    while(true) {
+    while(emergencyOff != 1) {
         myQuadcopter.readSensorValues();
 
         myQuadcopter.controller();
@@ -31,6 +36,8 @@
         motor_3=motorsPwm.m3;
         motor_4=motorsPwm.m4;
 
+
+
         // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r",  motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);
 
         Thread::wait(10);
@@ -48,13 +55,24 @@
 void battery_thread(void const *args)
 {
     float threshold_voltage = 13.0; // desired lowest battery voltage
+    float emergencyVoltage = 12.5; // switch off motors below it
     float max_voltage = 14.8; // max voltage level of battery
     float saturating_voltage = 18.38; // voltage at which ADC == 1
     float max_adc = 0.80522; // when battery is at 14.8V
     float threshold_adc = max_adc * threshold_voltage / max_voltage;
+    float emergency_adc = max_adc * emergencyVoltage / max_voltage;
+
     while(true) {
         if (battery.read() < threshold_adc) {
             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;
+            }
         }
         Thread::wait(1000);  // wait for some number of miliseconds
     }
@@ -108,7 +126,7 @@
 // TODO assign priorities to threads, test if it really works as we expect
     Thread thread(controller_thread);
     Thread threadR(rc_thread);
-    //Thread battery(battery_thread);
+    Thread battery(battery_thread);
 
     pc.printf("Entering control loop\n\r");