src/lib/BLE/devBLE.cpp

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