BLE unreliable when multiple servos are attached

I am trying to control a proof of concept quadcopter via BLE but I noticed that the Bluetooth connection was unreliable, often dropping off and being unable to be reconnected to. After testing the components one-by-one, I have isolated the problem to having 4 servo objects attached in my attempt to control the 4 ESCs. When only one servo is present (the rest commented out), the connection is fine even when 10m away with walls in-between. However when I introduce the rest of the servos, the connection drops off in minutes and cannot be reconnected to, sometimes the device cannot even be found the first time.

#include "Servo.h"
#include "ArduinoBLE.h"
#include "LSM9DS1.h"

Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;

BLEService commandsService("1101");
BLEShortCharacteristic rollPitchReferenceChar("2101", BLERead | BLEWrite);
BLEShortCharacteristic yawThrottleReferenceChar("2102", BLERead | BLEWrite);

BLEService sensorService("1102");
BLECharacteristic gyroReadingChar("2201", BLERead | BLENotify, 12, true);
BLECharacteristic accelReadingChar("2202", BLERead | BLENotify, 12, true);

typedef struct {
  unsigned char value1;
  unsigned char value2;
} cmdDataStruct;

typedef union {
  cmdDataStruct data;
  short byteArray;
} cmdDataPacket;

typedef struct {
  float x;
  float y;
  float z;
} floatVector;

typedef union {
  floatVector vector;
  byte byteArray[12];
} floatVectorPacket;

unsigned long previousMillis = 0;  

float freq = 104;
unsigned long invFreqMillis = ceil(1000/freq);

void setup() {
  Serial.begin(9600);    // initialize serial communication
  while (!Serial);

  pinMode(LED_BUILTIN, OUTPUT); // initialize the built-in LED pin to indicate when a central is connected

  // begin initialization
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");

    while (1);
  }

  BLE.debug(Serial);

  if (!IMU.begin()) {
    Serial.println("IMU Failed to initialize");

    while (1);
  }

  esc1.attach(5, 1000, 2000);
  esc2.attach(6, 1000, 2000);
  esc3.attach(9, 1000, 2000);
  esc4.attach(10, 1000, 2000);

  esc1.writeMicroseconds(1000);
  esc2.writeMicroseconds(1000);
  esc3.writeMicroseconds(1000);
  esc4.writeMicroseconds(1000);

  BLE.setLocalName("Module");

  BLE.setAdvertisedService(commandsService); // add the service UUID

  BLE.setEventHandler(BLEConnected, onConnectHandler);
  BLE.setEventHandler(BLEDisconnected, onDisconnectHandler);

  commandsService.addCharacteristic(rollPitchReferenceChar);
  commandsService.addCharacteristic(yawThrottleReferenceChar);
  BLE.addService(commandsService);
  
  {
    cmdDataPacket cmdData;
    cmdData.data.value1 = 128;
    cmdData.data.value2 = 128;
    rollPitchReferenceChar.writeValue(cmdData.byteArray);
    cmdData.data.value2 = 0;
    yawThrottleReferenceChar.writeValue(cmdData.byteArray);
  }
  
  sensorService.addCharacteristic(gyroReadingChar);
  sensorService.addCharacteristic(accelReadingChar);
  BLE.addService(sensorService);

  BLE.advertise();

}

void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= invFreqMillis) {
    Serial.println(currentMillis - previousMillis);
    previousMillis = currentMillis;
    

    if (BLE.central().connected()) {
      if (IMU.gyroscopeAvailable()) {
        float gx, gy, gz;

        IMU.readGyroscope(gx, gy, gz);

        floatVectorPacket packet;
        packet.vector.x = gx;
        packet.vector.y = gy;
        packet.vector.z = gz;
        gyroReadingChar.writeValue(packet.byteArray, 12);
      }
    }

    int val1, val2, val3, val4;
    {
    cmdDataPacket packet;
    rollPitchReferenceChar.readValue(packet.byteArray);
    val1 = map(packet.data.value1, 0, 256, 1000, 2000);
    val2 = map(packet.data.value2, 0, 256, 1000, 2000); 
    yawThrottleReferenceChar.readValue(packet.byteArray);
    val3 = map(packet.data.value1, 0, 256, 1000, 2000);
    val4 = map(packet.data.value2, 0, 256, 1000, 2000); 
    }
    esc1.writeMicroseconds(val1);
    esc2.writeMicroseconds(val2);
    esc3.writeMicroseconds(val3);
    esc4.writeMicroseconds(val4);
  }
}

void onConnectHandler(BLEDevice central) {
  Serial.print("Connected to ");
  Serial.println(central.address());
  digitalWrite(LED_BUILTIN, HIGH);
}

void onDisconnectHandler(BLEDevice central) {
  Serial.print("Disconnected from ");
  Serial.println(central.address());
  digitalWrite(LED_BUILTIN, LOW);
}

I was wondering if anyone had any workarounds or explanations as to why this is happening so that I may try to find a way to solve this problem.

So anyway, an update for anyone who is interested in connecting multiple servos while maintaining a stable BLE connection.

Use the mbed PWM library which you can find at PwmOut - Handbook | Mbed, or refer to my example below:

#include "Arduino.h"
//#include "Servo.h"
#include "ESC.h"

ESC::ESC(int pinNo, int minPulseWidth, int maxPulseWidth) :
esc(digitalPinToPinName(pinNo))
{
  _pinNo = pinNo;
  // Standard 1000ms for the default 50Hz PWM for ESCS
  // The reason why they are variables is a relic of some older design choices
  _minPulseWidth = minPulseWidth;
  // Standard 2000ms
  _maxPulseWidth = maxPulseWidth;
}

void ESC::Attach()
{
//  servo.attach(_pinNo, _minPulseWidth, _maxPulseWidth);
  // Gives a PWM frequency of 50Hz (period of 20ms)
  esc.period(0.020);
}

void ESC::Send(double commandedValue)
{
  // Just to map a desired value of 0 - 100 to the min and max pulse length
  int microseconds = map(commandedValue, 0, 100, _minPulseWidth, _maxPulseWidth);
//	servo.writeMicroseconds(map(commandedValue, 0, 100, _minPulseWidth, _maxPulseWidth));
  
  // Calculate the duty cycle corresponding to the pulse length and period
  float dutyCycle = microseconds / (float)20000;
  esc.write(dutyCycle);
}

// For calibration/arming of the ESCs
void ESC::sendMinPulse() {
  float dutyCycle = _minPulseWidth / (float)20000;
  esc.write(dutyCycle);
}

void ESC::sendMaxPulse() {
  float dutyCycle = _maxPulseWidth / (float)20000;
  esc.write(dutyCycle);
}

You use them as you would otherwise by initializing the ESC objects as below and calling the appropriate methods.

ESC esc1(D5, 1000, 2000);
ESC esc2(D6, 1000, 2000);
ESC esc3(D9, 1000, 2000);
ESC esc4(D10, 1000, 2000);

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.