From d17bc0fc4bb057378fadf3f9feb0de1df60d611a Mon Sep 17 00:00:00 2001 From: Jakob Stendahl Date: Mon, 11 Jan 2021 13:41:18 +0100 Subject: :sparkles: Add working bluetooth receiver --- source/HoverBitController.cpp | 182 ++++++++++++++++++++++++++++ source/HoverBitController.h | 68 +++++++++++ source/Screen.cpp | 41 +++++++ source/Screen.h | 73 ++++++++++++ source/main.cpp | 269 ++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 633 insertions(+) create mode 100644 source/HoverBitController.cpp create mode 100644 source/HoverBitController.h create mode 100644 source/Screen.cpp create mode 100644 source/Screen.h create mode 100644 source/main.cpp (limited to 'source') 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 +#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((float)Pitch * scaling + (float)offset); + unsigned int rollS = static_cast((float)Roll * scaling + (float)offset); + unsigned int yawS = static_cast((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; +} diff --git a/source/HoverBitController.h b/source/HoverBitController.h new file mode 100644 index 0000000..4963cd1 --- /dev/null +++ b/source/HoverBitController.h @@ -0,0 +1,68 @@ +/* +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. +*/ +#ifndef HOVERBITCONTROLLER_H_ +#define HOVERBITCONTROLLER_H_ + +#include + +#define BATTERY_LOW_LIMIT 3500 + +class HoverBitController { + private: + MicroBit* uBit; + + int buzzer; + int servo_1; + int arm; + int roll; + int pitch; + int yaw; + int throttle; + int failSafeC; + + bool mainController; + bool batteryEmpty; + int batteryMilliVolt; + float batteryFactor; + + public: + void init(MicroBit* _uBit); + void failSafe(void); + unsigned int getBatteryVoltage(void); + void AirBit(int Pitch,int Arm,int Roll,int Throttle,int Yaw,int Aux1,int Aux2); + void HoverControl(); + + int Throttle(); + void Throttle(int _throttle); + int Servo1(); + void Servo1(int _servo1); + int Roll(); + void Roll(int _roll); + bool Arm(); + void Arm(bool _arm); + bool BatteryEmpty(); +}; + +#endif // HOVERBITCONTROLLER_H_ diff --git a/source/Screen.cpp b/source/Screen.cpp new file mode 100644 index 0000000..c778798 --- /dev/null +++ b/source/Screen.cpp @@ -0,0 +1,41 @@ +/* +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 "Screen.h" + +/** + * Method for plotting a line, gotten from wonder-bit-source. + */ +void plotYLine(MicroBit *uBit, int y1, int y2, int x) { + if (y1 >= y2) { + for (int y = y2; y <= y1; y++) { + (*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); + } + } +} diff --git a/source/Screen.h b/source/Screen.h new file mode 100644 index 0000000..b60ba2b --- /dev/null +++ b/source/Screen.h @@ -0,0 +1,73 @@ +/* +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. +*/ +#ifndef SCREEN_H_ +#define SCREEN_H_ + +#include + +enum DisplayMainScreenMode { GRAPHS, BATTERY, OFF }; + +const char* const strBattDead = "\ + 000,255,255,255,000\n\ + 255,000,255,000,255\n\ + 255,255,255,255,255\n\ + 000,255,000,255,000\n\ + 000,255,000,255,000\n"; +const char* const strBattLow = "\ + 000,000,255,000,000\n\ + 000,255,255,255,000\n\ + 000,255,000,255,000\n\ + 000,255,000,255,000\n\ + 000,255,255,255,000\n"; +static const char* const strBattLevel[] = { + "\ + 000,000,255,000,000\n\ + 000,255,000,255,000\n\ + 000,255,000,255,000\n\ + 000,255,000,255,000\n\ + 000,255,255,255,000\n", + "\ + 000,000,255,000,000\n\ + 000,255,000,255,000\n\ + 000,255,000,255,000\n\ + 000,255,255,255,000\n\ + 000,255,255,255,000\n", + "\ + 000,000,255,000,000\n\ + 000,255,000,255,000\n\ + 000,255,255,255,000\n\ + 000,255,255,255,000\n\ + 000,255,255,255,000\n", + "\ + 000,000,255,000,000\n\ + 000,255,255,255,000\n\ + 000,255,255,255,000\n\ + 000,255,255,255,000\n\ + 000,255,255,255,000\n" +}; + +void plotYLine(MicroBit *uBit, int y1, int y2, int x); + +#endif // SCREEN_H_ diff --git a/source/main.cpp b/source/main.cpp new file mode 100644 index 0000000..7687b45 --- /dev/null +++ b/source/main.cpp @@ -0,0 +1,269 @@ +/* +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 +#include "MicroBitUARTService.h" +#include "HoverBitController.h" +#include "Screen.h" + +MicroBit uBit; +MicroBitUARTService *uart; +HoverBitController controller; + +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; + uBit.audio.setVolume(255); + 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); + char command = msg.substring(0, 1).toCharArray()[0]; + int value = atoi(msg.substring(1, msg.length() - 1).toCharArray()); + + if (command == 'R') { + controller.Roll(value); + if (displayMainScreenMode == OFF) { + uBit.display.scroll(controller.Roll()); + } + } else if (command == 'T') { + controller.Throttle(value); + if (displayMainScreenMode == OFF) { + uBit.display.scroll(controller.Throttle()); + } + } else if (command == 'A') { + controller.Arm(value == 1); + if (displayMainScreenMode == OFF) { + uBit.display.scroll(controller.Arm()); + } + } else if (command == 'S') { + controller.Servo1(value); + if (displayMainScreenMode == OFF) { + uBit.display.scroll(controller.Servo1()); + } + } else { + uBit.display.scroll(command); + } + } + +} + +void onDisconnected(MicroBitEvent) { + bConnected = 0; + 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); + } + } + 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() { + // uBit.display.clear(); + bool bDelayElapsed = (uBit.systemTime() - tmpTimer) > 1000; + if (bDelayElapsed) { tmpTimer = uBit.systemTime(); } + + 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); + } + } + 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); + } + } +} + +void onButtonA_press(MicroBitEvent e) { + controller.Roll(controller.Roll() + 3); +} +void onButtonB_press(MicroBitEvent e) { + controller.Roll(controller.Roll() - 3); +} + +int main() { + uBit.init(); + 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); + + // 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); + + // 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); + + uBit.audio.soundExpressions.play(ManagedString("hello")); + + 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(); + } + + controller.HoverControl(); + uBit.sleep(20); + } + + // If main exits, there may still be other fibers running or registered event handlers etc. + // Simply release this fiber, which will mean we enter the scheduler. Worse case, we then + // sit in the idle task forever, in a power efficient sleep. + release_fiber(); +} -- cgit v1.2.3