src/lib/BLE/devBLE.cpp

   1 #include "devBLE.h"
   2
   3 #if defined(PLATFORM_ESP32)
   4
   5 #include "common.h"
   6 #include "CRSF.h"
   7 #include "POWERMGNT.h"
   8 #include "hwTimer.h"
   9 #include "logging.h"
  10
  11 #include <BleGamepad.h>
  12 #include <NimBLEDevice.h>
  13
  14 class ELRSGamepad : public BleGamepad {
  15     public:
  16         ELRSGamepad() : BleGamepad("ExpressLRS Joystick", "ELRS", 100) {};
  17
  18     protected:
  19         void onStarted(NimBLEServer *pServer) {
  20             NimBLEDevice::setPower(ESP_PWR_LVL_P9);
  21         }
  22 };
  23
  24 static ELRSGamepad *bleGamepad;
  25
  26 void BluetoothJoystickUpdateValues()
  27 {
  28     if (bleGamepad->isConnected())
  29     {
  30         // map first 8 channels to axis
  31         int16_t data[8];
  32         for (uint8_t i = 0; i < 8; i++)
  33         {
  34             data[i] = map(ChannelData[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX, 0, 32767);
  35         }
  36         bleGamepad->setAxes(data[0], data[1], data[4], data[5], data[2], data[3], data[6], data[7]);
  37
  38         // map other 8 channels to buttons
  39         for (uint8_t i = 8; i < 16; i++)
  40         {
  41             if (ChannelData[i] >= CRSF_CHANNEL_VALUE_2000) {
  42                 bleGamepad->press(i - 7);
  43             } else {
  44                 bleGamepad->release(i - 7);
  45             }
  46         }
  47
  48         // send BLE report
  49         bleGamepad->sendReport();
  50     }
  51 }
  52
  53 void BluetoothJoystickBegin()
  54 {
  55     // bleGamepad is null if it hasn't been started yet
  56     if (bleGamepad != nullptr)
  57         return;
  58
  59     // construct the BLE immediately to prevent reentry from events/timeout
  60     bleGamepad = new ELRSGamepad();
  61
  62     POWERMGNT::setPower(MinPower);
  63     Radio.End();
  64     CRSF::RCdataCallback = BluetoothJoystickUpdateValues;
  65
  66     BleGamepadConfiguration *gamepadConfig = new BleGamepadConfiguration();
  67     gamepadConfig->setAutoReport(false);
  68
  69     DBGLN("Starting BLE Joystick!");
  70     bleGamepad->begin(gamepadConfig);
  71 }
  72
  73 static int timeout()
  74 {
  75     BluetoothJoystickBegin();
  76     return DURATION_NEVER;
  77 }
  78
  79 static int event()
  80 {
  81     if (connectionState == bleJoystick) {
  82         hwTimer::stop();
  83         return 200;
  84     }
  85     return DURATION_NEVER;
  86 }
  87
  88 device_t BLE_device = {
  89   .initialize = NULL,
  90   .start = NULL,
  91   .event = event,
  92   .timeout = timeout
  93 };
  94
  95 #endif