:sparkles: Add working bluetooth receiver

1 files changed, 182 insertions, 0 deletions

diff --git a/source/HoverBitController.cpp b/source/HoverBitController.cpp
new file mode 100644
index 0000000..65ebf17
--- /dev/null
+++ b/source/HoverBitController.cpp
@@ -0,0 +1,182 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2016 British Broadcasting Corporation.
+This software is provided by Lancaster University by arrangement with the BBC.
+
+Permission is hereby granted, free of charge, to any person obtaining a
+copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+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;
+    mainController = false;
+    batteryEmpty   = false;
+    batteryMilliVolt = 3700;
+    batteryFactor   = 4.42;
+
+    buzzer  = 0;
+    servo_1 = 0;
+    arm = 0;
+    roll = 0;
+    yaw = 0;
+    throttle = 0;
+    failSafeC = 0;
+
+    /* 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);
+    int o;
+    for (o = 0; o < 20; o++) {
+        AirBit(-90, 0, 90, 0, 90, 0, 0);
+        (*uBit).sleep(20);
+    }
+}
+
+/**
+ * This is not implemented yet.
+ */
+void HoverBitController::failSafe(void) {
+    // throttle = 0;
+    // roll = 0;
+    // yaw = 0;
+    // arm = 0;
+    // failSafeC++;
+}
+
+/**
+ * This returns the current voltage of the battery.
+ */
+unsigned int HoverBitController::getBatteryVoltage() {
+    float batteryFactor = 4.42;
+    int batteryMilliVolt = 3700;
+    return ((float)((&(*uBit).io.P0)->getAnalogValue()) * batteryFactor * 0.05) + ((float)batteryMilliVolt * 0.95);
+}
+
+/**
+ * Method for sending commands to the AirBit-card,
+ * this code is translated from the ts-code in MakeKit's original hex-file.
+ *
+ * Control TYPR12 (Throttle, Yaw, Pitch, Roll and AUX1 and AUX2) using the Spektsat 2048 protocol
+ * Throttle min: 0, max: 100
+ * Yaw, Pitch Roll: min -90, max 90
+ * Arm: 0 = Disarm, 1 = Arm
+ * Aux1: 0 - 180
+ * Aux2: 0 - 180
+ */
+void HoverBitController::AirBit(int Pitch,int Arm,int Roll,int Throttle,int Yaw,int Aux1,int Aux2) {
+    uint8_t buf[16];
+    float scaling = 1024 / 180;
+    int offset = 512;
+    float scalingServo = 1024 / 90;
+
+    unsigned int armS = 0;
+    if (Arm == 0) { armS = 0; }
+    if (Arm == 1) { armS = 1023; }
+
+    Pitch = - Pitch;
+    unsigned int aux1S     = Aux1  * scalingServo;
+    unsigned int aux2S     = Aux2  * scalingServo;
+    unsigned int pitchS    = static_cast<unsigned int>((float)Pitch * scaling + (float)offset);
+    unsigned int rollS     = static_cast<unsigned int>((float)Roll  * scaling + (float)offset);
+    unsigned int yawS      = static_cast<unsigned int>((float)Yaw   * scaling + (float)offset);
+    unsigned int throttleS = (Throttle * 512) / 50;
+    if (Throttle == 0) { throttleS = 0; }
+
+    if (aux1S > 1023) { aux1S = 1023; }
+    if (aux2S > 1023) { aux2S = 1023; }
+
+    if (throttleS > 1023) { throttleS = 1023; }
+    if (yawS > 1023) { yawS = 1023; }
+    if (pitchS > 1023) { pitchS = 1023; }
+    if (rollS > 1023) { rollS = 1023; }
+
+    // Header "Fade" (Spektsat code)
+    buf[0] = 0;
+    // Header "System" (Spektsat code)
+    buf[1] = 0x01;
+    // 0x01 22MS 1024 DSM2
+    // 0x12 11MS 2048 DSM2
+    // 0xa2 22MS 2048 DSMS
+    // 0xb2 11MS 2048 DSMX
+    buf[2]  = (0 << 2) | ((rollS >> 8) & 3);
+    buf[3]  = rollS & 255;
+    buf[4]  = (1 << 2) | ((pitchS >> 8) & 3);
+    buf[5]  = pitchS & 255;
+    buf[6]  = (2 << 2) | ((throttleS >> 8) & 3);
+    buf[7]  = throttleS & 255;
+    buf[8]  = (3 << 2) | ((yawS >> 8) & 3);
+    buf[9]  = yawS & 255;
+    buf[10] = (4 << 2) | ((armS >> 8) & 3);
+    buf[11] = armS & 255;
+    buf[12] = (5 << 2) | ((aux1S >> 8) & 3);
+    buf[13] = aux1S & 255;
+    buf[14] = (6 << 2) | ((aux2S >> 8) & 3);
+    buf[15] = aux2S & 255;
+    (*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);
+}
+
+int HoverBitController::Throttle() {
+    return throttle;
+}
+void HoverBitController::Throttle(int _throttle) {
+    if      (_throttle > 99) { throttle = 100; }
+    else if (_throttle <  0) { throttle =   0; }
+    else { throttle = _throttle; }
+}
+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() {
+    return roll;
+}
+void HoverBitController::Roll(int _roll) {
+    if      (_roll >   90) { roll =  90; }
+    else if (_roll <  -90) { roll = -90; }
+    else    { roll = _roll; }
+}
+bool HoverBitController::Arm() {
+    return (arm == 1);
+}
+void HoverBitController::Arm(bool _arm) {
+    arm = (int)_arm;
+}
+bool HoverBitController::BatteryEmpty() {
+    return batteryEmpty;
+}