Page 1 of 1

Bad connection bewteen BLE controller and ESP32

Posted: Fri Nov 26, 2021 4:55 pm
by AyubowanPro
Hello,

I'm using a BLE controller to control my ESP32. I'm using this library: https://github.com/nkolban/ESP32_BLE_Arduino.
I modified the file FreeRTOS.cpp of the ESP32 package to have a fixed time semaphore as Github users advised it. I replaced all occurencies of xSemaphoreTake(m_semaphore, portMAX_DELAY) by xSemaphoreTake(m_semaphore, 15000UL).

The thing is the ESP32 is struggling to connect to my controller. Sometimes, it connects perfectly but It often gets stuck at line 370. Here is the output of the serial monitor:
17:51:03.046 -> BLE Advertised Device found: Name: VR BOX, Address: ff:ff:20:01:b5:1e, serviceUUID: 00001812-0000-1000-8000-00805f9b34fb
17:51:03.046 -> Found VRBOX Server
17:51:03.046 -> Server has HID service
17:51:03.046 -> Forming a connection to ff:ff:20:01:b5:1e
17:51:03.046 -> - Created client
17:51:03.146 -> lld_pdu_get_tx_flush_nb HCI packet count mismatch (0, 1)
17:51:03.146 -> - Connected to server
17:51:03.146 -> Debug flag test 1

Here is the code:
  1. //==============================================================================
  2. // Robot controlled by VR Box using BLE on an ESP32
  3. // Target:   DoIt ESP32 DEVKIT V1, ESP-VROOM-32 module
  4. // Compiler: Arduino 1.8.10
  5. // SDK:      ESP32 Arduino SDK by Expressif Ver 1.0.4
  6. //==============================================================================
  7. // ESP32 (DoIt ESP32 DEVKIT V1, ESP-VROOM-32 module) board
  8. // In Arduino Board Manager, Select the following;
  9. // Arduino Board: "ESP32 Dev Module"
  10. // Flash Mode: QIO
  11. // Flash Size: 4MB (32Mb)
  12. // Flash Frequency: "80MHz"
  13. // Upload Speed: 921600
  14. // Core Debug Level: None
  15. //******************************************************************************
  16.  
  17. // VRBOX is a handheld Bluetooth BLE device with a joystick and six useful
  18. // buttons. This code shows how to setup the ESP32 as a BLE client to work
  19. // with the VRBOX BLE server. Most of the code is generic to any BLE server.
  20. // The name of the BLE server to search for and the services and characteristics
  21. // are specific to the VRBOX server.
  22. // You can use this code as a primer on how to connect other BLE servers to the
  23. // ESP32 or as the basis of any ESP32 project that you want to control using the
  24. // VRBOX device. The code should work with any ESP32 module.
  25.  
  26. // The VR BOX server will shut itself off if there is no activity for 5 minutes.
  27. // This will cause the ESP32 to lose the Bluetooth connection and do a reset.
  28.  
  29. // I bought mine at the local 5 Below store. The box is labeled; "Spektrum VR Control
  30. // Bluetooth RemoteController". I have also seen similar looking devices on the
  31. // Internet for upwards of $20.00. "VR BOX" is the name the device uses to identify
  32. // itself to other bluetooth devices.
  33.  
  34. // Each defined task requires its own dedicated RAM allocation for its stack
  35. // How much RAM to allocate is determined by how nested the task is and how
  36. // many variables are stored on the stack (local variables). The default size
  37. // defined here is for 5K * 4 bytes/32 bit word, so 20K bytes
  38. // If you are getting;
  39. // JGuru Meditation Error: Core  0 panoc'd (Unhandled debug exception)
  40. // Debug excaption reason: Stack canary watchpoint triggered (task name)
  41. // then increase the TaskStackSize by 1024 until the Stack canary errors stop.
  42. // The ESP32 VROOM module has 288K bytes of RAM.
  43. #define TaskStackSize   5120
  44.  
  45. // The blue and green LEDs are used to indicate when the ESP32 is scanning for
  46. // BLE servers and when the ESP32 has connected to the BLE server. The LEDs
  47. // can be moved to any GPIO pin (except LED, that is the builtin blue LED) that
  48. // is not input only.
  49.  
  50. #include "BLEDevice.h"
  51. #include "ESP32Servo.h"
  52. #include <Wire.h>
  53. #include <TinyPICO.h>
  54. #include <math.h>
  55.  
  56. // Initialise the TinyPICO library
  57. TinyPICO tp = TinyPICO();
  58.  
  59. //------ VR Box Definitions -----
  60. enum
  61. {
  62.   VB_TRIGGERS = 0,
  63.   VB_JOYX,
  64.   VB_JOYY,
  65.   VB_BTNAB,
  66.   VB_BTNCD,
  67.   VB_NUMBYTES
  68. };
  69.  
  70. // ===== VR Box Button Masks =====
  71. #define VB_LOW_TRIGGER    0x01
  72. #define VB_UPR_TRIGGER    0x02
  73. #define VB_BUTTON_A       0x01
  74. #define VB_BUTTON_B       0x02
  75. #define VB_BUTTON_C       0x01
  76. #define VB_BUTTON_D       0x02
  77. #define FRESHFLAG         0x80
  78.  
  79. #define JOYTIMEOUT        30      // joystick no activity timeout in mS
  80.  
  81. #define JoyStickDeadZone  0
  82.  
  83. #define ServerName  "VR BOX"      // change this if your server uses a different name
  84.  
  85. // ===== VRBOX Modes =====
  86. // This code assumes you are using the Mouse Mode
  87. // @ + A -> Music & Video mode
  88. // @ + B -> Horizontal Game mode
  89. // @ + C -> Vertical Game mode
  90. // @ + D -> Mouse Mode  // use this mode
  91. //  4 byte notification, Trigger Sws, Joystick X, Joystick Y, 0x00
  92.  
  93. // All four modes send data. However each mode uses different byte positions and
  94. // values for each of the switches. The joystick acts like a 'D' pad when not in
  95. // Mouse Mode (no analog value).
  96.  
  97. //==============================================================================
  98.  
  99. // I2C transmission
  100. const byte i2cSlaveAddress = 0x4;
  101. const int i2cFrequency = 100000;
  102. byte returnI2c; // 0:success | 1:data too long to fit in transmit buffer | 2:received NACK on transmit of address | 3:received NACK on transmit of data | 4:other error
  103. volatile bool flagButtonA = false;
  104. volatile bool flagButtonB = false;
  105. volatile bool flagButtonC = false;
  106. volatile bool flagButtonD = false;
  107.  
  108. //Dotstar LED of the TinyPico board
  109. long ledColor = 0;
  110. bool ledBlink = false;
  111. bool ledFlash = false;
  112. const byte ledBlinkPeriod = 1; // LED blink period in seconds
  113. #define green 0x00FF00
  114. #define red 0xFF0000
  115. #define blue 0x0000FF
  116. #define yellow 0xFFFF00
  117. const byte tinyPicoLedBrightness = 255;
  118.  
  119. // Servo gimbal
  120. // Le servo D645MW de Hitec, se contrôle avec un signal PWM allant de 900 à 2100us avec un neutre à 1500us.
  121. Servo servoGimbal;
  122. const byte servoPin = 5;
  123.  
  124. const float servoMax = 2090;
  125. const float servoMin = 920;
  126. const float servoNeutral = 1500;
  127. float servoPWM = servoNeutral;
  128.  
  129. const signed short inverseAxi = 1; // 1:normal | -1:inverse
  130. const byte power = 3; // Joystick equation : y = inverseAxi * servoPWMFactor * x^power
  131. const float servoPWMFactor = 0.001;
  132.  
  133. // Delays
  134. const unsigned int delayTaskServo = 0;
  135. const unsigned int delayTaskI2c = 10;
  136.  
  137. //==============================================================================
  138. typedef void (*NotifyCallback)(BLERemoteCharacteristic*, uint8_t*, size_t, bool);
  139.  
  140. // BLE address we are looking for
  141. static uint8_t VrBoxAddress[6] = {0xff,0xff,0x20,0x01,0xb5,0x1e};
  142.  
  143. // this is the service UUID of the VR Control handheld mouse/joystick device (HID)
  144. static BLEUUID serviceUUID("00001812-0000-1000-8000-00805f9b34fb");
  145.  
  146. // Battery Service UUID
  147. static BLEUUID BatteryServiceUUID("0000180F-0000-1000-8000-00805f9b34fb");
  148.  
  149. // this characteristic UUID works for joystick & triggers (report)
  150. static BLEUUID ReportCharUUID("00002A4D-0000-1000-8000-00805f9b34fb"); // report
  151.  
  152.  
  153. static boolean doConnect = false;
  154. static boolean connected = false;
  155. static BLERemoteCharacteristic* pRemoteCharacteristic;
  156. static BLEAdvertisedDevice* myDevice;
  157.  
  158. static BLERemoteCharacteristic* pBatRemoteCharacteristic;
  159.  
  160.  
  161. // pointer to a list of characteristics of the active service,
  162. // sorted by characteristic UUID
  163. std::map<std::string, BLERemoteCharacteristic*> *pmap;
  164. std::map<std::string, BLERemoteCharacteristic*> :: iterator itr;
  165.  
  166. // pointer to a list of characteristics of the active service,
  167. // sorted by characteristic handle
  168. std::map<std::uint16_t, BLERemoteCharacteristic*> *pmapbh;
  169. std::map<std::uint16_t, BLERemoteCharacteristic*> :: iterator itrbh;
  170.  
  171. // storage for pointers to characteristics we want to work with
  172. // to do: change to linked list ?
  173. BLERemoteCharacteristic *bleRcs[4];
  174.  
  175. // This is where we store the data from the buttons and joystick
  176. volatile byte   VrBoxData[VB_NUMBYTES];
  177. volatile bool   flag = false;         // indicates new data to process
  178.  
  179. // joyTimer is a 30 millisecond re-triggerable timer that sets the joystick
  180. // back to center if no activity on the joystick or trigger buttons.
  181. volatile uint32_t joyTimer = millis();
  182.  
  183. // tasks handles  
  184. TaskHandle_t HandleJS = NULL;       // handle of the joystick task
  185. TaskHandle_t HandleAB = NULL;       // handle of the A/B button task
  186. TaskHandle_t HandleCD = NULL;       // handle of the C/D button task
  187. TaskHandle_t HandleSCAN = NULL;     // handle of the BLE scan task
  188. TaskHandle_t HandleTINYLED = NULL;  // handle of the DotStar LED task
  189. TaskHandle_t HandleSERVO = NULL;    // handle of the Servo task
  190. TaskHandle_t HandleI2C = NULL;      // handle of the I2c task
  191.  
  192. // tasks priorities
  193. const short taskJoyStickPriority = 1;
  194. const short taskButtonABPriority = 1;
  195. const short taskButtonCDPriority = 1;
  196. const short taskScanPriority = 1;
  197. const short taskTinyLedPriority = 1;
  198. const short taskServoPriority = 1;
  199. const short taskI2cPriority = 1;
  200.  
  201. char bfr[80];
  202.  
  203. //******************************************************************************
  204. // HID notification callback handler.
  205. //******************************************************************************
  206. static void notifyCallback(
  207.   BLERemoteCharacteristic* pBLERemoteCharacteristic,
  208.   uint8_t* pData,
  209.   size_t length,
  210.   bool isNotify)
  211. {
  212. //  ledColor = yellow;
  213. //  ledFlash = true;
  214. //  ledBlink = false;
  215.  
  216.   Serial.print("Notify callback for characteristic ");
  217.  
  218.   Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
  219.   Serial.print(" of data length ");
  220.   Serial.println(length);
  221.   Serial.print("data: ");
  222.  
  223.   for (int i = 0; i < length; i++)
  224.     Serial.printf("%02X ", pData[i]);
  225.   Serial.println();
  226.  
  227.   // we are getting the two trigger buttons in the first byte, joyX & joyY in 2nd & 3rd bytes
  228.   // A four byte report is the joystick/trigger buttons.
  229.   // A two byte report is either the A/B buttons or the C/D buttons
  230.   // Low nibble equal to 0x05 indicates A/B buttons.
  231.   // A/B buttons auto-repeat if held. No other buttons do this.
  232.   if (4 == length)
  233.   {
  234.     // copy data to VrBoxData
  235.     for (int i = VB_TRIGGERS; i < VB_BTNAB; i++)
  236.       VrBoxData[i] = pData[i];
  237.  
  238.     // wake up the joystick/trigger buttons handler task
  239.     if (HandleJS)
  240.       vTaskResume(HandleJS);
  241.      
  242.     // restart the joystick timer
  243.     joyTimer = millis() + JOYTIMEOUT;
  244.   }
  245.   else if (2 == length)
  246.   {
  247.     // show the received data
  248.     if (pData[1] == 0x50)
  249.     {
  250.  
  251.       // A/B button report, wake the A/B button handler task
  252.       VrBoxData[VB_BTNAB] = pData[0] & 0x0F;
  253.       if (HandleAB)
  254.         vTaskResume(HandleAB);
  255.     }
  256.     else
  257.     {
  258.       // C/D button report, wake the C/D button handler task
  259.       VrBoxData[VB_BTNCD] = pData[0];
  260.       if (HandleCD)
  261.         vTaskResume(HandleCD);
  262.     }
  263.   }
  264. } //  notifyCallback
  265.  
  266. //******************************************************************************
  267. // Battery notification callback handler.
  268. //******************************************************************************
  269. static void BatteryNotifyCallback(
  270.   BLERemoteCharacteristic* pBLERemoteCharacteristic,
  271.   uint8_t* pData,
  272.   size_t length,
  273.   bool isNotify)
  274.  {
  275.   Serial.println("Battery Notification Callback Event");
  276.   Serial.print("Data length ");
  277.   Serial.println(length);
  278.   Serial.print("data: ");
  279.  
  280.   for (int i = 0; i < length; i++)
  281.     Serial.printf("%02X ", pData[i]);
  282.     Serial.println();
  283.  }  // BatteryNotifyCallback
  284.  
  285. //******************************************************************************
  286. // Connection state change event callback handler.
  287. //******************************************************************************
  288. class MyClientCallback : public BLEClientCallbacks
  289. {
  290.   void onConnect(BLEClient* pclient)
  291.   {
  292.     Serial.println("onConnect event");
  293.   }
  294.  
  295.   void onDisconnect(BLEClient* pclient)
  296.   {
  297.     Serial.println("onDisconnect event");
  298.    
  299.     connected = false;
  300.   }
  301. };
  302.  
  303. //******************************************************************************
  304. // Connect to a service, register for notifications from Report Characteristics.
  305. //******************************************************************************
  306. bool setupCharacteristics(BLERemoteService* pRemoteService, NotifyCallback pNotifyCallback)
  307. {
  308.   // get all the characteristics of the service using the handle as the key
  309.   pmapbh = pRemoteService->getCharacteristicsByHandle();
  310.  
  311.   // only interested in report characteristics that have the notify capability
  312.   for (itrbh = pmapbh->begin(); itrbh != pmapbh->end(); itrbh++)
  313.   {
  314.     BLEUUID x = itrbh->second->getUUID();
  315.     Serial.print("Characteristic UUID: ");
  316.     Serial.println(x.toString().c_str());
  317.     // the uuid must match the report uuid
  318.  
  319.     if (ReportCharUUID.equals(itrbh->second->getUUID()))
  320.     {
  321.       // found a report characteristic
  322.       Serial.println("Found a report characteristic");
  323.  
  324.       if (itrbh->second->canNotify())
  325.       {
  326.         Serial.println("Can notify");
  327.         // register for notifications from this characteristic
  328.         itrbh->second->registerForNotify(pNotifyCallback);
  329.  
  330.         sprintf(bfr, "Callback registered for: Handle: 0x%08X, %d", itrbh->first, itrbh->first);
  331.         Serial.println(bfr);
  332.       }
  333.       else
  334.       {
  335.         Serial.println("No notification");
  336.       }
  337.     }
  338.     else
  339.     {
  340.         sprintf(bfr, "Found Characteristic UUID: %s\n", itrbh->second->getUUID().toString().c_str());
  341.         Serial.println(bfr);
  342.     }
  343.   } //  for
  344. } // setupCharacteristics
  345.  
  346. //******************************************************************************
  347. // Validate the server has the correct name and services we are looking for.
  348. // The server must have the HID service, the Battery Service is optional.
  349. //******************************************************************************
  350. bool connectToServer()
  351. {
  352.   Serial.print("Forming a connection to ");
  353.   Serial.println(myDevice->getAddress().toString().c_str());
  354.  
  355.   BLEClient*  pClient  = BLEDevice::createClient();
  356.   Serial.println(" - Created client");
  357.  
  358.   pClient->setClientCallbacks(new MyClientCallback());
  359.  
  360.   // Connect to the remote BLE Server.
  361.   pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
  362.   Serial.println(" - Connected to server");
  363.  
  364.   // BLE servers may offer several services, each with unique characteristics
  365.   // we can identify the type of service by using the service UUID
  366.  
  367.   // Obtain a reference to the service we are after in the remote BLE server.
  368.   // this will return a pointer to the remote service if it has a matching service UUID
  369.   Serial.println("Debug flag test 1");
  370.   BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
  371.   Serial.println("Debug flag test 2");
  372.   if (pRemoteService == nullptr)
  373.   {
  374.     Serial.print("Failed to find HID service UUID: ");
  375.     Serial.println(serviceUUID.toString().c_str());
  376.     pClient->disconnect();
  377.     return false;
  378.   }
  379.  
  380.   Serial.println(" - Found HID service");
  381.   setupCharacteristics(pRemoteService, notifyCallback);
  382.   pRemoteService = pClient->getService(BatteryServiceUUID);
  383.  
  384.   if (pRemoteService == nullptr)
  385.   {
  386.     Serial.print("Failed to find battery service UUID: ");
  387.     Serial.println(serviceUUID.toString().c_str());
  388.   }
  389.   else
  390.   {
  391.     Serial.println(" - Found battery service");
  392.     setupCharacteristics(pRemoteService, BatteryNotifyCallback);
  393.   }
  394.  
  395.   connected = true;
  396. } //  connectToServer
  397.  
  398. //******************************************************************************
  399. // Scan for BLE servers and find the first one that advertises the service we are looking for.
  400. //******************************************************************************
  401. class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks
  402. {
  403.   // Called for each advertising BLE server.
  404.   void onResult(BLEAdvertisedDevice advertisedDevice)
  405.   {
  406.     Serial.print("BLE Advertised Device found: ");
  407.     Serial.println(advertisedDevice.toString().c_str());
  408.  
  409.     //Check if the server has the correct BLE address
  410.     if (advertisedDevice.getAddress().equals(VrBoxAddress))
  411.     {
  412.       Serial.println("Found VRBOX Server");
  413.  
  414.       // we found a server with the correct name, see if it has the service we are
  415.       // interested in (HID)
  416.  
  417.       if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID))
  418.       {
  419.         Serial.println("Server has HID service");
  420.        
  421.         BLEDevice::getScan()->stop();
  422.  
  423.         myDevice = new BLEAdvertisedDevice(advertisedDevice);
  424.         doConnect = true;
  425.        
  426.       } // Found our server
  427.       else
  428.       {
  429.         Serial.println("Server does not have an HID service, not our server");
  430.       }
  431.     }
  432.     else
  433.     {
  434.       Serial.println("Server address, not our server");
  435.      
  436.     }
  437.   } // onResult
  438. }; // MyAdvertisedDeviceCallbacks
  439.  
  440.  
  441. // All of these tasks are designed to run forever. The tasks are resumed when
  442. // a notification message is received with new data.
  443. //******************************************************************************
  444. // Joystick handler Task.
  445. // Moving the joystick off center causes this task to be resumed about every
  446. // 15ms. Press or release of either trigger button will also resume this task.
  447. // If this task does not complete in less than 15mS you will lose joystick
  448. // movement data !!!
  449. // Holding the lower button will prevent the server from detecting that the
  450. // upper button has been pressed.
  451. // Holding the upper trigger and pressing the lower trigger results in the server
  452. // sending a notification that the lower trigger is pressed (the upper trigger
  453. // will be zero!). Releasing the lower trigger will cause the server to send a
  454. // notification that the upper trigger is pressed and the lower trigger is
  455. // released.
  456. //******************************************************************************
  457. void taskJoyStick(void *parameter)
  458. {
  459.   int8_t  x;
  460.   int8_t  y;
  461.   uint8_t triggers;
  462.  
  463.   //===== if the task requires any one time initialization, put it here =====
  464.  
  465.   // forever loop
  466.   while(true)
  467.   {
  468.     // give up the CPU, wait for new data
  469.     vTaskSuspend(NULL);
  470.  
  471.     // Serial.print("taskJoyStick running on core ");
  472.     // Serial.println(xPortGetCoreID());
  473.  
  474.     // we just woke up, new data is available, convert joystick data to
  475.     // signed 8 bit integers
  476.     x = (int8_t)VrBoxData[VB_JOYX];
  477.     y = (int8_t)VrBoxData[VB_JOYY]; //Compris entre -24 et 24
  478.     triggers = VrBoxData[VB_TRIGGERS];
  479.    
  480.     Serial.printf("Joystick X: %d, Y: %d Triggers: %02X\n", x, y, triggers);
  481.  
  482.     if (y < -JoyStickDeadZone)
  483.     {
  484.       // move forward
  485.       Serial.println("Forward");
  486.  
  487.       // nacelle en avant
  488.       servoPWM = constrain(servoPWM - (pow((float) y, power) * inverseAxi * servoPWMFactor), servoMin, servoMax);
  489.       Serial.print("y * f: ");
  490.       Serial.println((float) y*servoPWMFactor);
  491.       Serial.println(servoPWM);
  492.     }
  493.     else if (y > JoyStickDeadZone)
  494.     {
  495.       // move backward
  496.       Serial.println("Backward");
  497.  
  498.       // nacelle en arrière
  499.       servoPWM = constrain(servoPWM - (pow((float) y, power) * inverseAxi * servoPWMFactor), servoMin, servoMax);
  500.       Serial.print("y * f: ");
  501.       Serial.println((float) y*servoPWMFactor);
  502.       Serial.println(servoPWM);
  503.     }
  504.        
  505.     if (x > JoyStickDeadZone)
  506.     {
  507.       // turn right
  508.       Serial.println("Turn Right");
  509.  
  510.       //===== add your code here =====
  511.      
  512.     }
  513.     else if (x < -JoyStickDeadZone)
  514.     {
  515.       // turn left
  516.       Serial.println("Turn Left");
  517.  
  518.       //===== add your code here =====
  519.      
  520.     }
  521.  
  522.     if (triggers & VB_LOW_TRIGGER)
  523.     {
  524.       // the lower trigger button is pressed
  525.       Serial.println("Low Trigger Pressed");
  526.  
  527.       // nacelle à 90°
  528.       servoPWM = servoMin;
  529.     }
  530.  
  531.     if (triggers & VB_UPR_TRIGGER)
  532.     {
  533.       // the upper trigger button is pressed
  534.       Serial.println("Upper Trigger Pressed");
  535.  
  536.       // nacelle au neutre
  537.       servoPWM = servoNeutral;
  538.     }
  539.   } //  for
  540. } //  taskJoyStick
  541.  
  542. //******************************************************************************
  543. // A & B Buttons handler Task.
  544. // Holding the A or B button down will cause this task to be invoked about every
  545. // 15ms. If this task does not complete within 15mS you will lose button events.
  546. // The AB buttons work similar to the trigger buttons in that the A button will
  547. // prevent the B button from being detected and will override the B button when
  548. // pressed while the B button is held down.
  549. //******************************************************************************
  550.  
  551. void taskButtonAB(void *parameter)
  552. {
  553.   uint8_t buttons;
  554.   unsigned long pastTime = 0;
  555.  
  556.   //===== if the task requires any one time initialization, put it here =====
  557.  
  558.   while(true)
  559.   {
  560.     // give up the CPU, wait for new data
  561.     vTaskSuspend(NULL);
  562.  
  563.     // Serial.print("taskButtonAB running on core ");
  564.     // Serial.println(xPortGetCoreID());
  565.    
  566.     // we just woke up, new data is available
  567.     buttons = VrBoxData[VB_BTNAB];
  568.     Serial.printf("A/B Buttons: %02X\n", buttons);
  569.        
  570.     if (buttons & VB_BUTTON_A)
  571.     {
  572.       // button A pressed or is being held down      
  573.       // Lumière +
  574.       Serial.println("Button A");
  575.       flagButtonA = true;
  576.     }
  577.  
  578.     if (buttons & VB_BUTTON_B)
  579.     {
  580.       // button B pressed or is being held down
  581.       // Lumière -
  582.       Serial.println("Button B");
  583.       flagButtonB = true;
  584.     }
  585.   } //  for
  586. } //  taskButtonAB
  587.  
  588. //******************************************************************************
  589. // C & D Buttons handler Task.
  590. // Press or release of either the C or D button will resume this task. Holding
  591. // one button down blocks the Server from detecting the other button being
  592. // pressed.
  593. //******************************************************************************
  594. void taskButtonCD(void *parameter)
  595. {
  596.   uint8_t buttons;
  597.  
  598.   //===== if the task requires any one time initialization, put it here =====
  599.  
  600.   while(true)
  601.   {
  602.     // give up the CPU
  603.     vTaskSuspend(NULL);
  604.  
  605.     // Serial.print("taskButtonCD running on core ");
  606.     // Serial.println(xPortGetCoreID());
  607.  
  608.     // we just woke up, new data is available
  609.     buttons = VrBoxData[VB_BTNCD];
  610.     Serial.printf("C/D Buttons: %02X\n", buttons);
  611.    
  612.     if (buttons & VB_BUTTON_C)
  613.     {
  614.       // button C pressed
  615.       Serial.println("Button C");
  616.      
  617.       // Lumière max
  618.       flagButtonC = true;
  619.     }
  620.  
  621.     if (buttons & VB_BUTTON_D)
  622.     {
  623.       // button D pressed
  624.       Serial.println("Button D");
  625.  
  626.       // Lumière min
  627.       flagButtonD = true;
  628.     }
  629.   } //  for
  630. } //  taskButtonCD
  631.  
  632. //******************************************************************************
  633.  
  634. void taskScan(void *parameter)
  635. {
  636.  
  637.   while(true)
  638.   {
  639.     // give up the CPU
  640.     vTaskSuspend(NULL);
  641.  
  642.     // Serial.print("taskScan running on core ");
  643.     // Serial.println(xPortGetCoreID());
  644.    
  645.     // we just woke up
  646.     Serial.println("Start scanning...");
  647.     BLEDevice::getScan()->stop(); // in case a scan is already running (it prevents the library from bugging) // Maybe it can be deleted
  648.     BLEDevice::getScan()->start(0);
  649.   }
  650. }
  651.  
  652. //static void scanCompleteCB(BLEScanResults scanResults)
  653. //{
  654. //  printf("Scan complete!\n");
  655. //  scanResults.dump();
  656. //}
  657.  
  658. //******************************************************************************
  659.  
  660. void taskServo(void *parameter)
  661. {
  662.   vTaskSuspend(NULL);
  663.  
  664.   while(true)
  665.   {
  666.     vTaskDelay(delayTaskServo);
  667.     // Serial.print("taskServo running on core ");
  668.     // Serial.println(xPortGetCoreID());
  669.     servoGimbal.writeMicroseconds(servoPWM);
  670.     Serial.print("PWM: ");
  671.     Serial.println(servoPWM);
  672.   }
  673. }
  674.  
  675. //******************************************************************************
  676.  
  677. void taskTinyLed(void *parameter) {
  678.   while(true) {
  679.     // Serial.println("taskTinyLed");
  680.     tp.DotStar_Clear();
  681.     tp.DotStar_SetPixelColor( ledColor );
  682.     tp.DotStar_SetBrightness( tinyPicoLedBrightness );
  683.     tp.DotStar_Show();
  684.     while(!ledBlink) { //Standby
  685.       // Serial.println("LED Standby");
  686.       vTaskDelay(10);
  687.     }
  688.  
  689.     if(ledBlink) {
  690.       vTaskDelay((ledBlinkPeriod*1000)/2);
  691.       tp.DotStar_Clear();
  692.       tp.DotStar_SetBrightness( 0 );
  693.       tp.DotStar_Show();
  694.       vTaskDelay((ledBlinkPeriod*1000)/2);
  695.     }
  696.   }
  697. }
  698.  
  699. void taskI2c(void *parameter) {
  700.  
  701.   // give up the CPU
  702.     vTaskSuspend(NULL);
  703.  
  704.   while(true) {
  705.    
  706.     if(flagButtonA == true) {
  707.       Wire.beginTransmission(i2cSlaveAddress);
  708.       Wire.write(0);
  709.       returnI2c = Wire.endTransmission(i2cSlaveAddress);
  710.       Serial.println(returnI2c);
  711.       Serial.println("i2c 0");
  712.       flagButtonA = false;
  713.     }
  714.  
  715.     if(flagButtonB == true) {
  716.       Wire.beginTransmission(i2cSlaveAddress);
  717.       Wire.write(1);
  718.       returnI2c = Wire.endTransmission(i2cSlaveAddress);
  719.       Serial.println(returnI2c);
  720.       Serial.println("i2c 1");
  721.       flagButtonB = false;
  722.     }
  723.  
  724.     if(flagButtonC == true) {
  725.       Wire.beginTransmission(i2cSlaveAddress);
  726.       Wire.write(2);
  727.       returnI2c = Wire.endTransmission(i2cSlaveAddress);
  728.       Serial.println(returnI2c);
  729.       Serial.println("i2c 2");
  730.       flagButtonC = false;
  731.     }
  732.  
  733.     if(flagButtonD == true) {
  734.       Wire.beginTransmission(i2cSlaveAddress);
  735.       Wire.write(3);
  736.       returnI2c = Wire.endTransmission(i2cSlaveAddress);
  737.       Serial.println(returnI2c);
  738.       Serial.println("i2c 3");
  739.       flagButtonD = false;
  740.     }
  741.  
  742.     vTaskDelay(delayTaskI2c);
  743.   }
  744. }
  745.  
  746. //******************************************************************************
  747. void setup()
  748. {
  749.   //==============================================================================
  750.   Wire.begin(); // Rejoindre le bus I2C (Pas besoin d adresse pour le maitre)
  751.   Wire.setClock(i2cFrequency);
  752.  
  753.   // Attach the channel to the GPIO to be controlled
  754.   //ledcAttachPin(FemtoBuckPin, femToBuckChannel);
  755.   //==============================================================================
  756.   // Servo gimbal
  757.   servoGimbal.attach(servoPin);
  758.   //==============================================================================
  759.  
  760.   BaseType_t xReturned;
  761.  
  762.   Serial.begin(115200);
  763.  
  764.   // create tasks to handle the joystick and buttons
  765.   xReturned = xTaskCreatePinnedToCore(taskJoyStick,             // task to handle activity on the joystick.
  766.                                       "Joystick",               // String with name of task.
  767.                                       TaskStackSize,            // Stack size in 32 bit words.
  768.                                       NULL,                     // Parameter passed as input of the task
  769.                                       taskJoyStickPriority,     // Priority of the task.
  770.                                       &HandleJS,                // Task handle.
  771.                                       1);                       // Core where the task should run
  772.   if (pdPASS == xReturned)
  773.   {
  774.     Serial.println("Joystick Task Created");
  775.   }
  776.  
  777.   xReturned = xTaskCreatePinnedToCore(taskButtonAB,             // task to handle activity on the A & B buttons.
  778.                                       "ButtonsAB",              // String with name of task.
  779.                                       TaskStackSize,            // Stack size in 32 bit words.
  780.                                       NULL,                     // Parameter passed as input of the task
  781.                                       taskButtonABPriority,     // Priority of the task.
  782.                                       &HandleAB,                // Task handle.
  783.                                       1);                       // Core where the task should run
  784.   if (pdPASS == xReturned)
  785.   {
  786.     Serial.println("AB Button Task Created");
  787.    
  788.   }
  789.  
  790.   xReturned = xTaskCreatePinnedToCore(taskButtonCD,             // task to handle activity on the C & D buttons.
  791.                                       "ButtonsCD",              // String with name of task.
  792.                                       TaskStackSize,            // Stack size in 32 bit words.
  793.                                       NULL,                     // Parameter passed as input of the task
  794.                                       taskButtonCDPriority,     // Priority of the task.
  795.                                       &HandleCD,                // Task handle.
  796.                                       1);                       // Core where the task should run
  797.   if (pdPASS == xReturned)
  798.   {
  799.     Serial.println("CD Button Task Created");
  800.    
  801.   }
  802.  
  803.   // create task to handle the BLE scan
  804.   xReturned = xTaskCreatePinnedToCore(taskScan,
  805.                                       "Scan",
  806.                                       TaskStackSize,
  807.                                       NULL,
  808.                                       taskScanPriority,
  809.                                       &HandleSCAN,
  810.                                       1);
  811.   if (pdPASS == xReturned)
  812.   {
  813.     Serial.println("Scan task created");
  814.    
  815.   }
  816.  
  817.   // create task to handle the Servo control of the COMETE
  818.   xReturned = xTaskCreatePinnedToCore(taskServo,
  819.                                       "Servo",
  820.                                       TaskStackSize,
  821.                                       NULL,
  822.                                       taskServoPriority,
  823.                                       &HandleSERVO,
  824.                                       1);
  825.   if (pdPASS == xReturned)
  826.   {
  827.     Serial.println("Servo task created");
  828.    
  829.   }
  830.  
  831.   // create task to handle the DotStar LED
  832.   xReturned = xTaskCreatePinnedToCore(taskTinyLed,
  833.                                       "TinyLed",
  834.                                       TaskStackSize,
  835.                                       NULL,
  836.                                       taskTinyLedPriority,
  837.                                       &HandleTINYLED,
  838.                                       1);
  839.   if (pdPASS == xReturned)
  840.   {
  841.     Serial.println("DotStar LED task created");
  842.    
  843.   }
  844.  
  845.   // create task to handle the i2c communication
  846.   xReturned = xTaskCreatePinnedToCore(taskI2c,
  847.                                       "i2c",
  848.                                       TaskStackSize,
  849.                                       NULL,
  850.                                       taskI2cPriority,
  851.                                       &HandleI2C,
  852.                                       1);
  853.   if (pdPASS == xReturned)
  854.   {
  855.     Serial.println("i2c task created");
  856.    
  857.   }
  858.  
  859.   Serial.println("Starting ESP32 BLE Client...");
  860.   BLEDevice::init("");
  861.  
  862.   // Retrieve a GATT Scanner and set the callback we want to use to be informed
  863.   // when we have detected a new device.  Specify that we want active scanning
  864.   BLEScan* pBLEScan = BLEDevice::getScan();
  865.   pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  866.   pBLEScan->setInterval(1349);
  867.   pBLEScan->setWindow(449);
  868.   pBLEScan->setActiveScan(true);
  869.  
  870. } // End of setup.
  871.  
  872. //******************************************************************************
  873. // This is the Arduino main loop function.
  874. //******************************************************************************
  875. void loop()
  876. {
  877.   // If the flag "doConnect" is true then we have scanned for and found the desired
  878.   // BLE Server with which we wish to connect.  Now we connect to it.  Once we are
  879.   // connected we set the connected flag to be true.
  880.   if (doConnect == true)
  881.   {
  882.     if (connectToServer())
  883.     {
  884.       Serial.println("We are now connected to the BLE Server.");
  885.  
  886.       // wake up the servo task
  887.       Serial.println("Resume servo task");
  888.       vTaskResume(HandleSERVO);
  889.       vTaskResume(HandleI2C);
  890.  
  891.       // Green LED
  892.       ledColor = green;
  893.       ledBlink = false;
  894.       ledFlash = false;
  895.     }
  896.     else
  897.     {
  898.       Serial.println("We have failed to connect to the server; there is nothin more we will do.");
  899.     }
  900.     doConnect = false;
  901.   }
  902.  
  903.   if (connected)
  904.   {
  905.     // joystick no activity detector
  906.     if (joyTimer && (joyTimer < millis()))
  907.     {
  908.       Serial.println("Timeout");
  909.       // no joystick notification for 30mS, center the joystick
  910.       VrBoxData[VB_JOYX] = VrBoxData[VB_JOYY] = 0;
  911.  
  912.       // wake up the joystick task
  913.       vTaskResume(HandleJS);
  914.      
  915.       joyTimer = 0;
  916.     }
  917.   }
  918.   else
  919.   {
  920.     //Start BLE scan
  921.     vTaskResume(HandleSCAN);
  922.  
  923.     //Stop the servo task
  924.     //Serial.println("Stop servo task");
  925.     vTaskSuspend(HandleSERVO);
  926.     vTaskSuspend(HandleI2C);
  927.  
  928.     //Blink DotStar LED
  929.     ledColor = blue;
  930.     ledBlink = true;
  931.     ledFlash = false;
  932.   }
  933. } // End of loop
Any idea would be appreciated ! :D