:sparkles: Add emptyBattery code

1 files changed, 19 insertions, 2 deletions

diff --git a/source/HoverBitController.cpp b/source/HoverBitController.cpp
index df9893e..1ecc21b 100644
--- a/source/HoverBitController.cpp
+++ b/source/HoverBitController.cpp
@@ -32,7 +32,7 @@ DEALINGS IN THE SOFTWARE.
  */
 void HoverBitController::init() {
     mainController = false;
-    batteryEmpty   = false;
+    bBatteryEmpty   = false;
     batteryMilliVolt = 3700;
     batteryFactor   = 4.42;
 
@@ -82,6 +82,18 @@ unsigned int HoverBitController::GetBatteryVoltage() {
 }
 
 /**
+ * Check wether battery level is too low.
+ */
+void HoverBitController::checkBattery() {
+    if (GetBatteryVoltage() < BATTERY_LOW_LIMIT - 60) {
+        bBatteryEmpty = true;
+        Throttle(0);
+        Arm(0);
+        Rudder(0);
+    }
+}
+
+/**
  * Method for sending commands to the AirBit-card,
  * this code is translated from the ts-code in MakeKit's original hex-file.
  *
@@ -148,6 +160,10 @@ void HoverBitController::AirBit(int Pitch,int Arm,int Roll,int Throttle,int Yaw,
  * Method that sends commands with the current values for all parameters.
  */
 void HoverBitController::HoverControl() {
+    checkBattery();
+    if (BatteryEmpty()) {
+        Arm(0);
+    }
     if (!failSafe()) {
         AirBit(0, arm, 0, throttle, roll, roll + 45, servo_1);
     }
@@ -181,5 +197,6 @@ void HoverBitController::Arm(bool _arm) {
     lastReceiveTime = uBit.systemTime();
 }
 bool HoverBitController::BatteryEmpty() {
-    return batteryEmpty;
+    checkBattery();
+    return bBatteryEmpty;
 }