:sparkles: Add emptyBattery code

2 files changed, 21 insertions, 3 deletions

diff --git a/inc/HoverBitController.h b/inc/HoverBitController.h
index 80ad173..f2c10b7 100644
--- a/inc/HoverBitController.h
+++ b/inc/HoverBitController.h
@@ -51,11 +51,12 @@ class HoverBitController {
         unsigned long lastReceiveTime;
 
         bool mainController;
-        bool batteryEmpty;
+        bool bBatteryEmpty;
         int batteryMilliVolt;
         float batteryFactor;
 
         bool failSafe(void);
+        void checkBattery();
         void AirBit(int Pitch,int Arm,int Roll,int Throttle,int Yaw,int Aux1,int Aux2);
 
     public:
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;
 }