Merge pull request #8 from JakobST1n/dev1.0.0

Dev

7 files changed, 414 insertions, 273 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ac32e4b..e2130f4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -43,6 +43,7 @@ sbeParseJson(codal codal_json)
 
 set(CODAL_APP_OUTPUT_DIR ".")
 set(CODAL_APP_SOURCE_DIR "source")
+set(CODAL_APP_INCLUDE_DIR "inc")
 
 if("${codal.application}" STRGREATER "")
     set(CODAL_APP_SOURCE_DIR "${codal.application}")
@@ -81,17 +82,6 @@ SET(CODAL_OUTPUT_NAME ${device.device})
 SET(CODAL_TARGET_PROCESSOR ${device.processor})
 SET(CODAL_TARGET_CPU_ARCHITECTURE ${device.architecture})
 
-# if this is the first build, lets copy a sample main.cpp from the target if available.
-if(NOT EXISTS ${CMAKE_CURRENT_LIST_DIR}/${CODAL_APP_SOURCE_DIR} AND EXISTS ${CMAKE_CURRENT_LIST_DIR}/${LIB_DEST}/${codal.target.name}/samples/main.cpp)
-    FILE(COPY ${CMAKE_CURRENT_LIST_DIR}/${LIB_DEST}/${codal.target.name}/samples/main.cpp DESTINATION ${CMAKE_CURRENT_LIST_DIR}/${CODAL_APP_SOURCE_DIR})
-endif()
-
-#copy samples and remove main.cpp
-if(NOT EXISTS ${CMAKE_CURRENT_LIST_DIR}/samples AND EXISTS ${CMAKE_CURRENT_LIST_DIR}/${LIB_DEST}/${codal.target.name}/samples/)
-    FILE(COPY ${CMAKE_CURRENT_LIST_DIR}/${LIB_DEST}/${codal.target.name}/samples DESTINATION ${CMAKE_CURRENT_LIST_DIR})
-    FILE(REMOVE ${CMAKE_CURRENT_LIST_DIR}/samples/main.cpp)
-endif()
-
 ####################
 
 SET(TOOLCHAIN ${device.toolchain})
@@ -231,7 +221,7 @@ if("${device.libraries}" STRGREATER "")
 endif()
 
 #finally, find sources and includes of the application, and create a target.
-RECURSIVE_FIND_DIR(INCLUDE_DIRS "./inc" "${PROJECT_SOURCE_DIR}/${CODAL_APP_SOURCE_DIR}" "*.h")
+RECURSIVE_FIND_DIR(INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/${CODAL_APP_INCLUDE_DIR}" "*.h")
 # *.c?? only catches .cpp, not .c, so let's be precise
 RECURSIVE_FIND_FILE(SOURCE_FILES "${PROJECT_SOURCE_DIR}/${CODAL_APP_SOURCE_DIR}" "*.cpp")
 
diff --git a/README.md b/README.md
index d3f197a..10119e1 100644
--- a/README.md
+++ b/README.md
@@ -1,13 +1,15 @@
 # HOVER:BIT Bluetooth Controller software
 This is software for controlling the HOVER:BIT kit with a progressive web app.
 
-The web app lives in the [controller](https://github.com/JakobST1n/hoverbit-ble/tree/controller) branch, you can access it [here](http://jakobst1n.github.io/hoverbit-ble/). It works the best if you install it on your device, usually you should get a prompt to install it when accessing the site. 
+The web app lives in the [controller](https://github.com/JakobST1n/hoverbit-ble/tree/controller) branch, you can access it [here](http://jakobst1n.github.io/hoverbit-ble/). It works the best if you install it on your device, usually you should get a prompt to install it when accessing the site.
 
-Only tested on __Google Chrome__! Will __NOT__ work on Safari (as apple doesn't allow web bluetooth). I have not tested on IOS, but it should work with chrome there as well.
+> Only tested on __Google Chrome__! Will __NOT__ work on Safari (as apple doesn't allow web bluetooth). I have not tested on IOS, but it should work with chrome there as well.
+
+The Micro:bit code for this project is not the most efficient, object oriented and readable code is highly prioritized instead.
 
 ## Usage
 ### Flashing the micro:bit
-Get the last [hex file](https://github.com/JakobST1n/hoverbit-ble/releases/latest) from Releases, and flash your micro:bit with this file. 
+Get the last [hex file](https://github.com/JakobST1n/hoverbit-ble/releases/latest) from Releases, and flash your micro:bit with this file.
 To do this, plug the micro:bit into your computer. The micro:bit will then appear as a removable flash drive, then simply copy (drag-drop)
 the hex-file into the flash-drive folder.
 
@@ -37,3 +39,4 @@ Take a look at [active issues](https://github.com/JakobST1n/hoverbit-ble/issues)
 
 ## Credits
 - Joysticks on webapp is from the [nipplejs](https://yoannmoi.net/nipplejs/) library.
+- [micro:bit V2 Runtime (Docs)](https://rneacy.dev/mbv2/ble/)
diff --git a/source/HoverBitController.h b/inc/HoverBitController.h
index 4963cd1..f2c10b7 100644
--- a/source/HoverBitController.h
+++ b/inc/HoverBitController.h
@@ -27,12 +27,20 @@ DEALINGS IN THE SOFTWARE.
 
 #include <MicroBit.h>
 
-#define BATTERY_LOW_LIMIT 3500
+#define BATTERY_LOW_LIMIT   3500
+#define FSAFE_TLIM_THROTTLE 1000 // When to cut the throttle
+#define FSAFE_TLIM_ARM      5000 // When to disarm
 
+extern MicroBit uBit;
+
+/**
+ * This class can be used to interface with a AirBit card for controlling a HOVER:BIT kit.
+ *
+ * A lot of the features of the airbit is ignored here and made easy to understand if all
+ * you want to do is use it for a hoverbit.
+ */
 class HoverBitController {
     private:
-        MicroBit* uBit;
-
         int buzzer;
         int servo_1;
         int arm;
@@ -40,26 +48,26 @@ class HoverBitController {
         int pitch;
         int yaw;
         int throttle;
-        int failSafeC;
+        unsigned long lastReceiveTime;
 
         bool mainController;
-        bool batteryEmpty;
+        bool bBatteryEmpty;
         int batteryMilliVolt;
         float batteryFactor;
 
-    public:
-        void init(MicroBit* _uBit);
-        void failSafe(void);
-        unsigned int getBatteryVoltage(void);
+        bool failSafe(void);
+        void checkBattery();
         void AirBit(int Pitch,int Arm,int Roll,int Throttle,int Yaw,int Aux1,int Aux2);
+
+    public:
+        void init();
+        unsigned int GetBatteryVoltage(void);
         void HoverControl();
 
         int  Throttle();
         void Throttle(int _throttle);
-        int  Servo1();
-        void Servo1(int _servo1);
-        int  Roll();
-        void Roll(int _roll);
+        int  Rudder();
+        void Rudder(int _rudder);
         bool Arm();
         void Arm(bool _arm);
         bool BatteryEmpty();
diff --git a/source/Screen.h b/inc/Screen.h
index b60ba2b..0ea7b8c 100644
--- a/source/Screen.h
+++ b/inc/Screen.h
@@ -26,6 +26,10 @@ DEALINGS IN THE SOFTWARE.
 #define SCREEN_H_
 
 #include <MicroBit.h>
+#include "HoverBitController.h"
+
+extern MicroBit uBit;
+extern HoverBitController controller;
 
 enum DisplayMainScreenMode { GRAPHS, BATTERY, OFF };
 
@@ -67,7 +71,40 @@ static const char* const strBattLevel[] = {
     000,255,255,255,000\n\
     000,255,255,255,000\n"
 };
+const char* const bluetoothSymbol = "\
+    000,000,255,255,000\n\
+    255,000,255,000,255\n\
+    000,255,255,255,000\n\
+    255,000,255,000,255\n\
+    000,000,255,255,000\n";
+
+class HoverBitDisplay {
+    private:
+        DisplayMainScreenMode screenMode;
+        unsigned int tmpTimer;
+        bool BLEconnected;
+        bool isPause;
+
+        void lowBattery();
+        void BLENotConnected();
+        void mainScreen();
+        void showGraphs();
+
+    public:
+        void mode(DisplayMainScreenMode mode);
+        DisplayMainScreenMode mode();
+        void nextMode();
+        void pause();
+        void pause(bool p);
+        void update();
+        void updateBLEState(bool connected);
+};
 
-void plotYLine(MicroBit *uBit, int y1, int y2, int x);
+void plotYLine(int y1, int y2, int x);
+void plotXLine(int x1, int x2, int y);
+void iconBatteryDead();
+void iconBatteryLow();
+void iconBatteryCharging();
+void batteryLevelFullScreen();
 
 #endif // SCREEN_H_
diff --git a/source/HoverBitController.cpp b/source/HoverBitController.cpp
index 65ebf17..301fdeb 100644
--- a/source/HoverBitController.cpp
+++ b/source/HoverBitController.cpp
@@ -22,19 +22,15 @@ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 DEALINGS IN THE SOFTWARE.
 */
-#include <MicroBit.h>
 #include "HoverBitController.h"
 
 /**
  * Init method for HoverBitController, this sets everything to the default values.
  * It also initializes the airbit-pcb with some protocol magic.
- *
- * @param _uBit the MicroBit instance
  */
-void HoverBitController::init(MicroBit* _uBit) {
-    uBit = _uBit;
+void HoverBitController::init() {
     mainController = false;
-    batteryEmpty   = false;
+    bBatteryEmpty   = false;
     batteryMilliVolt = 3700;
     batteryFactor   = 4.42;
 
@@ -44,37 +40,54 @@ void HoverBitController::init(MicroBit* _uBit) {
     roll = 0;
     yaw = 0;
     throttle = 0;
-    failSafeC = 0;
+    lastReceiveTime = uBit.systemTime();
 
-    /* I am not completly sure what this does, but it seems to me like this is
-       putting the air:bit board in some kind of "bind-mode", on the spec-sheet
-       there isn't any documentation for what 20 pulses means tho... */
-    (*uBit).sleep(100);
+    /* I am not completly sure what this does, according to the hover:bit guys
+       this is some magic to disable gyro-control of the lift fan. */
+    uBit.sleep(100);
     int o;
     for (o = 0; o < 20; o++) {
         AirBit(-90, 0, 90, 0, 90, 0, 0);
-        (*uBit).sleep(20);
+        uBit.sleep(20);
     }
 }
 
 /**
  * This is not implemented yet.
  */
-void HoverBitController::failSafe(void) {
-    // throttle = 0;
-    // roll = 0;
-    // yaw = 0;
-    // arm = 0;
-    // failSafeC++;
+bool HoverBitController::failSafe(void) {
+    unsigned long deltaReceiveTime = uBit.systemTime() - lastReceiveTime;
+    if (deltaReceiveTime > FSAFE_TLIM_ARM) {
+        arm = 0;
+        AirBit(0, 0, 0, 0, 0, 0 + 45, servo_1);
+    }
+    if (deltaReceiveTime > FSAFE_TLIM_THROTTLE) {
+        throttle = 0;
+        roll = 0;
+        AirBit(0, arm, 0, 0, 0, 0 + 45, servo_1);
+    }
+    return (deltaReceiveTime > FSAFE_TLIM_THROTTLE) || (deltaReceiveTime > FSAFE_TLIM_ARM);
 }
 
 /**
  * This returns the current voltage of the battery.
  */
-unsigned int HoverBitController::getBatteryVoltage() {
+unsigned int HoverBitController::GetBatteryVoltage() {
     float batteryFactor = 4.42;
     int batteryMilliVolt = 3700;
-    return ((float)((&(*uBit).io.P0)->getAnalogValue()) * batteryFactor * 0.05) + ((float)batteryMilliVolt * 0.95);
+    return ((float)((&uBit.io.P0)->getAnalogValue()) * batteryFactor * 0.05) + ((float)batteryMilliVolt * 0.95);
+}
+
+/**
+ * Check wether battery level is too low.
+ */
+void HoverBitController::checkBattery() {
+    if (GetBatteryVoltage() < BATTERY_LOW_LIMIT - 60) {
+        bBatteryEmpty = true;
+        throttle = 0;
+        arm = 0;
+        roll = 0;
+    }
 }
 
 /**
@@ -137,14 +150,20 @@ void HoverBitController::AirBit(int Pitch,int Arm,int Roll,int Throttle,int Yaw,
     buf[13] = aux1S & 255;
     buf[14] = (6 << 2) | ((aux2S >> 8) & 3);
     buf[15] = aux2S & 255;
-    (*uBit).serial.send(buf, 16, SYNC_SPINWAIT);
+    uBit.serial.send(buf, 16, SYNC_SPINWAIT);
 }
 
 /**
  * Method that sends commands with the current values for all parameters.
  */
 void HoverBitController::HoverControl() {
-    AirBit(0, arm, 0, throttle, roll, roll + 45, servo_1);
+    checkBattery();
+    if (BatteryEmpty()) {
+        arm = 0;
+    }
+    if (!failSafe()) {
+        AirBit(0, arm, 0, throttle, roll, roll + 45, servo_1);
+    }
 }
 
 int HoverBitController::Throttle() {
@@ -154,29 +173,27 @@ void HoverBitController::Throttle(int _throttle) {
     if      (_throttle > 99) { throttle = 100; }
     else if (_throttle <  0) { throttle =   0; }
     else { throttle = _throttle; }
+    lastReceiveTime = uBit.systemTime();
 }
-int HoverBitController::Servo1() {
-    return servo_1;
-}
-void HoverBitController::Servo1(int _servo1) {
-    if      (_servo1 > 180) { servo_1 = 180; }
-    else if (_servo1 <   0) { servo_1 =   0; }
-    else { servo_1 = _servo1; }
-}
-int HoverBitController::Roll() {
+int HoverBitController::Rudder() {
+    // The AirBit uses the roll parameter to control the hoverbit's rudder.
     return roll;
 }
-void HoverBitController::Roll(int _roll) {
-    if      (_roll >   90) { roll =  90; }
-    else if (_roll <  -90) { roll = -90; }
-    else    { roll = _roll; }
+void HoverBitController::Rudder(int _rudder) {
+    // The AirBit uses the roll parameter to control the hoverbit's rudder.
+    if      (_rudder >   90) { roll =  90; }
+    else if (_rudder <  -90) { roll = -90; }
+    else    { roll = _rudder; }
+    lastReceiveTime = uBit.systemTime();
 }
 bool HoverBitController::Arm() {
     return (arm == 1);
 }
 void HoverBitController::Arm(bool _arm) {
     arm = (int)_arm;
+    lastReceiveTime = uBit.systemTime();
 }
 bool HoverBitController::BatteryEmpty() {
-    return batteryEmpty;
+    checkBattery();
+    return bBatteryEmpty;
 }
diff --git a/source/Screen.cpp b/source/Screen.cpp
index c778798..f961994 100644
--- a/source/Screen.cpp
+++ b/source/Screen.cpp
@@ -24,18 +24,218 @@ DEALINGS IN THE SOFTWARE.
 */
 #include "Screen.h"
 
+DisplayMainScreenMode displayMainScreenMode = OFF;
+
 /**
  * Method for plotting a line, gotten from wonder-bit-source.
  */
-void plotYLine(MicroBit *uBit, int y1, int y2, int x) {
+void plotYLine(int y1, int y2, int x) {
     if (y1 >= y2) {
         for (int y = y2; y <= y1; y++) {
-            (*uBit).display.image.setPixelValue(x, y, 255);
+            uBit.display.image.setPixelValue(x, y, 255);
         }
     }
     else if (y1 < y2) {
         for (int y = y1; y <= y2; y++) {
-            (*uBit).display.image.setPixelValue(x, y, 255);
+            uBit.display.image.setPixelValue(x, y, 255);
+        }
+    }
+}
+
+/**
+ * Draw a line along the X axis
+ */
+void plotXLine(int x1, int x2, int y) {
+
+     if (x1 >= x2) {
+         for (int x = x2; x <= x1; x++) {
+             uBit.display.image.setPixelValue(x, y, 255);
+         }
+     }
+     else if (x1 < x2) {
+         for (int x = x1; x <= x2; x++) {
+             uBit.display.image.setPixelValue(x, y, 255);
+         }
+     }
+ }
+
+/**
+ * Display the dead battery icon.
+ */
+void iconBatteryDead() {
+    MicroBitImage img(strBattDead);
+    uBit.display.print(img);
+}
+
+/**
+ * Display the low battery icon.
+ */
+void iconBatteryLow() {
+    MicroBitImage img(strBattLow);
+    uBit.display.print(img);
+}
+
+/**
+ * Show a battery charging icon.
+ */
+void iconBatteryCharging() {
+    int batteryMilliVolt = controller.GetBatteryVoltage();
+    int low = 0;
+    int high = 3;
+    if (batteryMilliVolt >= 4200) {
+        low = 3;
+    } else if (batteryMilliVolt >= 4040) {
+        low = 2;
+    } else if (batteryMilliVolt >= 3900) {
+        low = 1;
+    }
+
+    for (int i = low; i <= high; i++) {
+        MicroBitImage img(strBattLevel[i]);
+        uBit.display.print(img);
+        uBit.sleep(400);
+    }
+}
+
+/**
+ * Display the appropriate battery icon based on battery voltage.
+ */
+void batteryLevelFullScreen() {
+    int batteryMilliVolt = controller.GetBatteryVoltage();
+    int level = 0;
+    if (controller.Arm()) {
+        level = (((batteryMilliVolt - 3400) * 3) / 500);
+    } else {
+        level = (((batteryMilliVolt - 3700) * 3) / 500);
+    }
+    if (level < 0) { level = 0; }
+    if (level > 3) { level = 3; }
+    MicroBitImage img(strBattLevel[level]);
+    uBit.display.print(img);
+}
+
+/**
+ * Get DisplayScreenMode.
+ */
+DisplayMainScreenMode HoverBitDisplay::mode() {
+    return screenMode;
+}
+
+/**
+ * Set DisplayScreenMode.
+ */
+void HoverBitDisplay::mode(DisplayMainScreenMode mode) {
+    screenMode = mode;
+}
+
+/**
+ * Go to the next HoverBitDisplay mode.
+ */
+void HoverBitDisplay::nextMode() {
+    uBit.display.clear();
+    switch (screenMode) {
+        case GRAPHS:
+            screenMode = BATTERY;
+            break;
+        case BATTERY:
+            screenMode = OFF;
+            break;
+        case OFF:
+            screenMode = GRAPHS;
+            break;
+    }
+}
+
+/**
+ * Pause the HoverBitDisplay module, this means that you are
+ * free to print what you want without this module interefering
+ */
+void HoverBitDisplay::pause() {
+    pause(true);
+}
+
+/**
+ * Turn on or off HoverBitDisplay pause.
+ */
+void HoverBitDisplay::pause(bool p) {
+    if (p) {
+        uBit.display.clear();
+    }
+    isPause = p;
+}
+
+/**
+ * Set BLE connected state, this determines if the bluetooth icon is
+ * shown on screen or not.
+ */
+void HoverBitDisplay::updateBLEState(bool connected) {
+    BLEconnected = connected;
+}
+
+/**
+ * This updates the matrix display to what should be displayed now.
+ */
+void HoverBitDisplay::update() {
+    if (isPause) { return; }
+    int batteryMilliVolt = controller.GetBatteryVoltage();
+
+    if ((((&uBit.io.P0)->getAnalogValue()) < 600) && (((&uBit.io.P0)->getAnalogValue()) >= 400)) {
+        iconBatteryCharging();
+    } else if (controller.BatteryEmpty() || (batteryMilliVolt < BATTERY_LOW_LIMIT && (&uBit.io.P0)->getAnalogValue() > 300)) {
+        lowBattery();
+    } else if (!BLEconnected) {
+        BLENotConnected();
+    } else {
+        showGraphs();
+    }
+}
+
+/**
+ * Displays dead or low battery icon.
+ */
+void HoverBitDisplay::lowBattery() {
+    if (controller.BatteryEmpty()) {
+        iconBatteryDead();
+    } else if (controller.GetBatteryVoltage() > BATTERY_LOW_LIMIT - 50) {
+        iconBatteryLow();
+    } else {
+        iconBatteryDead();
+    }
+}
+
+/**
+ * Called by HoverBitDisplay::update when BLEConnected is false.
+ * Flashes a bluetooth symbol on screen.
+ */
+void HoverBitDisplay::BLENotConnected() {
+    if (uBit.systemTime() % 1000 > 250) {
+        MicroBitImage img(bluetoothSymbol);
+        uBit.display.print(img);
+    } else {
+        uBit.display.clear();
+    }
+}
+
+/**
+ * Show the GRAPH displayMainScreenMode
+ */
+void HoverBitDisplay::showGraphs() {
+    uBit.display.clear();
+    if (controller.Arm()) {
+        if (uBit.systemTime() % 500 > 250) {
+            uBit.display.image.setPixelValue(0, 0, 255);
+        }
+    }
+    uBit.display.image.setPixelValue(0, (100 - controller.Throttle()) / 25, 255);
+    uBit.display.image.setPixelValue((45 - controller.Rudder()) / 18, 2, 255);
+    int batteryMilliVolt = controller.GetBatteryVoltage();
+    if (batteryMilliVolt > 100) {
+        if (controller.Arm()) {
+            plotYLine(0, (((batteryMilliVolt - 3400) * 4) / 500), 4);
+        } else {
+            plotYLine(0, (((batteryMilliVolt - 3700) * 4) / 500), 4);
         }
+    } else if (uBit.systemTime() % 500 > 250) {
+        uBit.display.image.setPixelValue(4, 4, 255);
     }
 }
diff --git a/source/main.cpp b/source/main.cpp
index caf8ffe..d2848c9 100644
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -27,261 +27,147 @@ DEALINGS IN THE SOFTWARE.
 #include "HoverBitController.h"
 #include "Screen.h"
 
+#define VERSION "1.0.0"
+#define BLE_UART_DELIM ":"
+
 MicroBit uBit;
 MicroBitUARTService *uart;
 HoverBitController controller;
+HoverBitDisplay hoverBitDisplay;
 
 bool bConnected = false;
-
-bool batteryEmpty = false;
 bool bCapLogoIsPressed = false;
-int batteryMilliVolt = 3700;
-unsigned long tmpTimer;
-bool bBLEIndicator = false;
-
-DisplayMainScreenMode displayMainScreenMode = GRAPHS;
 
 void onConnected(MicroBitEvent) {
     bConnected = 1;
+    hoverBitDisplay.updateBLEState(true);
     uBit.audio.soundExpressions.play(ManagedString("giggle"));
-
-    // mobile app will send ASCII strings terminated with the colon character
-    ManagedString eom(":");
-
-    while (bConnected) {
-        ManagedString msg = uart->readUntil(eom);
-        int length = msg.length();
-        const char* command = msg.toCharArray();
-
-        ManagedString accString("ACC:");
-
-        char cCommand = command[0];
-        char cChar;
-        int startI = 1;
-        bool bEOC = false;
-        int valLength = 0;
-
-        for (int i = 1; i < length; i++) {
-            cChar = command[i];
-
-            if (i >= length - 1) {
-                bEOC = true;
-                valLength = i - startI + 1;
-            } else if (cChar == 'R' || cChar == 'T' || cChar == 'A' || cChar == 'S') {
-                bEOC = true;
-                valLength = i - startI;
-            }
-
-            if (bEOC) {
-                /* We will just assume that we end up with a valid integer here */
-                int value = atoi(msg.substring(startI, startI + valLength).toCharArray());
-
-                if (cCommand == 'R') {
-                    controller.Roll(value);
-                    accString = accString + ManagedString("R") + ManagedString(controller.Roll());
-                } else if (cCommand == 'T') {
-                    controller.Throttle(value);
-                    accString = accString + ManagedString("T") + ManagedString(controller.Throttle());
-                } else if (cCommand == 'A') {
-                    controller.Arm(value == 1);
-                    accString = accString + ManagedString("A") + ManagedString(controller.Arm());
-                } else if (cCommand == 'S') {
-                    controller.Servo1(value);
-                    accString = accString + ManagedString("S") + ManagedString(controller.Servo1());
-                }
-
-                cCommand = cChar;
-                startI = i+1;
-                bEOC = false;
-            }
-        }
-        // @TODO: Move this to the hoverControl module, we would rather like to have that there, or in the main loop.
-        // it could also be in the same clause as the batttery sending, but we might want to have it more
-        // dependent on actual received values.
-        uart->send(accString);
-    }
-
 }
 
 void onDisconnected(MicroBitEvent) {
     bConnected = 0;
+    hoverBitDisplay.updateBLEState(false);
     uBit.audio.soundExpressions.play(ManagedString("sad"));
 }
 
-void iconBatteryDead() {
-    MicroBitImage img(strBattDead);
-    uBit.display.print(img);
-}
-
-void iconBatteryLow() {
-    MicroBitImage img(strBattLow);
-    uBit.display.print(img);
-}
-
-void lowBattery() {
-    if (batteryEmpty) {
-        iconBatteryDead();
-    } else if (batteryMilliVolt > BATTERY_LOW_LIMIT - 50){
-        iconBatteryLow();
-    } else {
-        iconBatteryDead();
-    }
-}
-
-void iconBatteryCharging() {
-    int low = 0;
-    int high = 3;
-    if (batteryMilliVolt >= 4200) {
-        low = 3;
-    } else if (batteryMilliVolt >= 4040) {
-        low = 2;
-    } else if (batteryMilliVolt >= 3900) {
-        low = 1;
-    }
-
-    for (int i = low; i <= high; i++) {
-        MicroBitImage img(strBattLevel[i]);
-        uBit.display.print(img);
-        uBit.sleep(400);
-    }
-}
-
-void batteryLevelFullScreen() {
-    int level = 0;
-    if (controller.Arm()) {
-        level = (((batteryMilliVolt - 3400) * 3) / 500);
-    } else {
-        level = (((batteryMilliVolt - 3700) * 3) / 500);
-    }
-    if (level < 0) { level = 0; }
-    if (level > 3) { level = 3; }
-    MicroBitImage img(strBattLevel[level]);
-    uBit.display.print(img);
-}
-
-void plotYLine(int y1, int y2, int x) {
-    /**
-     * Draw a line along the Y axis. y1: first pixel, y2: last pixel
-     */
-
-    if (y1 >= y2) {
-        for (int y = y2; y <= y1; y++) {
-            uBit.display.image.setPixelValue(x, y, 255);
+void onDelim(MicroBitEvent) {
+    ManagedString msg = uart->readUntil(BLE_UART_DELIM);
+    uBit.display.image.setPixelValue(1, 0, 255);
+
+    int length = msg.length();
+    const char* command = msg.toCharArray();
+
+    ManagedString accString("ACC:");
+
+    char cCommand = command[0];
+    char cChar;
+    int startI = 1;
+    bool bEOC = false;
+    int valLength = 0;
+
+    for (int i = 1; i < length; i++) {
+        cChar = command[i];
+
+        if (i >= length - 1) {
+            bEOC = true;
+            valLength = i - startI + 1;
+        } else if (cChar == 'R' || // Roll
+                   cChar == 'T' || // Throttle
+                   cChar == 'A' || // Arm
+                   cChar == 'S' || // (Servo1) Keeping this for compatability
+                   cChar == 'D'    // DisplayMainScreenMode
+                  ) {
+            bEOC = true;
+            valLength = i - startI;
         }
-    }
-    else if (y1 < y2) {
-        for (int y = y1; y <= y2; y++) {
-            uBit.display.image.setPixelValue(x, y, 255);
-        }
-    }
-}
-
-void nextMainScreenDisplayMode() {
-    uBit.display.clear();
-    switch (displayMainScreenMode) {
-        case GRAPHS:
-            displayMainScreenMode = BATTERY;
-            break;
-        case BATTERY:
-            displayMainScreenMode = OFF;
-            break;
-        case OFF:
-            displayMainScreenMode = GRAPHS;
-            break;
-    }
-}
-
-void mainScreen() {
-    bool bDelayElapsed = (uBit.systemTime() - tmpTimer) > 1000;
-    if (bDelayElapsed) { tmpTimer = uBit.systemTime(); }
 
-    if (bDelayElapsed && bConnected) { uart->send(ManagedString("B:") + ManagedString(batteryMilliVolt)); }
-
-    switch (displayMainScreenMode) {
-        case OFF:
-            break;
-        case BATTERY:
-            uBit.display.clear();
-            batteryLevelFullScreen();
-            break;
-        case GRAPHS:
-        default:
-            uBit.display.clear();
-            if (batteryMilliVolt > 100) {
-                if (controller.Arm()) {
-                    plotYLine(0, (((batteryMilliVolt - 3400) * 4) / 500), 4);
-                } else {
-                    plotYLine(0, (((batteryMilliVolt - 3700) * 4) / 500), 4);
+        if (bEOC) {
+            /* We will just assume that we end up with a valid integer here */
+            int value = atoi(msg.substring(startI, startI + valLength).toCharArray());
+
+            if (cCommand == 'R') {
+                controller.Rudder(value);
+                accString = accString + ManagedString("R") + ManagedString(controller.Rudder());
+            } else if (cCommand == 'T') {
+                controller.Throttle(value);
+                accString = accString + ManagedString("T") + ManagedString(controller.Throttle());
+            } else if (cCommand == 'A') {
+                controller.Arm(value == 1);
+                accString = accString + ManagedString("A") + ManagedString(controller.Arm());
+            } else if (cCommand == 'D') {
+                switch (value) {
+                    case 0:
+                        hoverBitDisplay.mode(GRAPHS);
+                        break;
+                    case 1:
+                        hoverBitDisplay.mode(BATTERY);
+                        break;
+                    case 2:
+                        hoverBitDisplay.mode(OFF);
+                        break;
                 }
+            } else {
+                // We ignore it :)
             }
-            break;
-    }
 
-    if (bConnected) {
-        uBit.display.image.setPixelValue(0, 0, 255);
-    } else {
-        if (bDelayElapsed) { bBLEIndicator = !bBLEIndicator; }
-        if (bBLEIndicator) {
-            uBit.display.image.setPixelValue(0, 0, 0);
-        } else {
-            uBit.display.image.setPixelValue(0, 0, 255);
+            cCommand = cChar;
+            startI = i+1;
+            bEOC = false;
         }
     }
+    // @TODO: Move this to the hoverControl module, we would rather like to have that there, or in the main loop.
+    // it could also be in the same clause as the batttery sending, but we might want to have it more
+    // dependent on actual received values.
+    uart->send(accString);
 }
 
-void onButtonA_press(MicroBitEvent e) {
-}
-void onButtonB_press(MicroBitEvent e) {
+void onButtonAB_press(MicroBitEvent e) {
+    hoverBitDisplay.pause();
+    uBit.display.scroll(VERSION);
+    hoverBitDisplay.pause(false);
 }
 
 int main() {
     uBit.init();
-    uBit.audio.setVolume(255);
-    tmpTimer = uBit.systemTime();
 
     // Setup serial for Spektsat communication with air:bit board
     uBit.serial.setBaud(115200);
     uBit.serial.redirect(uBit.io.P1, uBit.io.P2);
 
-    /* Initialize hover:bit controller module
-     * the init procedure have to be run within 100ms after air:bit power up */
-    controller.init(&uBit);
+    /* Initialize hover:bit controller module, these timeouts are some voodo 
+       magic I don't quite understand. But for the init method to function
+       as we want, there should be a relatively long delay here. */
+    uBit.sleep(3000);
+    controller.init();
 
     // Setup listeners
     uBit.messageBus.listen(MICROBIT_ID_BLE, MICROBIT_BLE_EVT_CONNECTED, onConnected);
     uBit.messageBus.listen(MICROBIT_ID_BLE, MICROBIT_BLE_EVT_DISCONNECTED, onDisconnected);
-    uBit.messageBus.listen(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK, onButtonA_press);
-    uBit.messageBus.listen(MICROBIT_ID_BUTTON_B, MICROBIT_BUTTON_EVT_CLICK, onButtonB_press);
+    uBit.messageBus.listen(MICROBIT_ID_BLE_UART, MICROBIT_UART_S_EVT_DELIM_MATCH, onDelim);
+
+    uBit.messageBus.listen(MICROBIT_ID_BUTTON_AB, MICROBIT_BUTTON_EVT_CLICK, onButtonAB_press);
 
     // uartService
     // Note GATT table size increased from default in MicroBitConfig.h
     // #define MICROBIT_SD_GATT_TABLE_SIZE             0x500
     uart = new MicroBitUARTService(*uBit.ble, 32, 32);
+    uart->eventOn(BLE_UART_DELIM);
 
+    uBit.audio.setVolume(20);
     uBit.audio.soundExpressions.play(ManagedString("hello"));
+    hoverBitDisplay.mode(GRAPHS);
 
     while (1) {
-        batteryMilliVolt = controller.getBatteryVoltage();
-
-        if (uBit.logo.isPressed()) {
-            if (!bCapLogoIsPressed) {
-                bCapLogoIsPressed = true;
-                nextMainScreenDisplayMode();
-            }
-        } else if (bCapLogoIsPressed ){
-            bCapLogoIsPressed = false;
-        }
-
-        if ((((&uBit.io.P0)->getAnalogValue()) < 600) && (((&uBit.io.P0)->getAnalogValue()) >= 400)) {
-            iconBatteryCharging();
-        } else if (controller.BatteryEmpty() || (batteryMilliVolt < BATTERY_LOW_LIMIT && (&uBit.io.P0)->getAnalogValue() > 300)) {
-            lowBattery();
-        } else {
-            mainScreen();
-        }
-
+        hoverBitDisplay.update();
         controller.HoverControl();
+        if (uBit.systemTime() % 2000 > 1900) {
+            uart->send(
+                ManagedString("B:")
+                + ManagedString((int)controller.GetBatteryVoltage())
+            );
+        }
         uBit.sleep(20);
+        schedule();
     }
 
     // If main exits, there may still be other fibers running or registered event handlers etc.