Bluetooth BLE ST shield sketch loop locks up

[jakehenry41kc]

I have a TinyDuino project where I am trying to operate 2 of the micro EMAX servos offered on TinyCircuits website over BLE using a smartphone app that I've developed. My problem is that if I send servo pulses to the BLE shield too fast, the entire sketch loop ceases to function and I have to hard reset the TinyDuino processor to get the sketch to run again. This issue does not appear to be related to servo libraries or attempting to move the servos. When I comment out all servo functionality inside the main loop() function the sketch still locks up if I send integers to the BLE ST shield too fast.

Stack:

• TinyDuino Processor Shield
• TinyDuino USB Shield
• TinyDuino TinyDuino BLE ST Shield
• TinyDuinos Servo Controller Shield

Sketch "BLEServoDriver.ino":

Code: [Select]

#include <SPI.h>
#include <STBLE.h>
#include <Wire.h>
#include <ServoDriver.h>
                               
//Debug output adds extra flash and memory requirements!
#ifndef BLE_DEBUG
#define BLE_DEBUG true
#endif

#if defined (ARDUINO_ARCH_AVR)
#define SerialMonitorInterface Serial
#elif defined(ARDUINO_ARCH_SAMD)
#define SerialMonitorInterface SerialUSB
#endif

uint8_t ble_rx_buffer[21];
uint8_t ble_rx_buffer_len = 0;
uint8_t ble_connection_state = false;
#define PIPE_UART_OVER_BTLE_UART_TX_TX 0
ServoDriver servoDriver(NO_R_REMOVED);//this value affects the I2C address, which can be changed by
                                //removing resistors R1-R3. Then the corresponding R1_REMOVED,
                                //R2_REMOVED, R1_R2_REMOVED, R1_R4_REMOVED and so on can be set.
                                //Default is NO_R_REMOVED

double range;
double midpoint;
double upper;
double lower;
double speed;

uint16_t oldPos = -1;

/* This method is called once at arduino boot time and is used for setup type tasks that must be carried out */
void setup()
{
  SerialMonitorInterface.begin(57600);
 
  range = 2000.0;
  midpoint = 1500.0;
  upper = midpoint + (range/2.0);
  lower = midpoint - (range/2.0);
  speed = 0.09/((range / 180.0) * 60.0);
 
  // Load Bluetooth drivers first
  loadBluetoothDrivers();
  // Load servo drivers second
  loadServoDrivers();
}

/* This method is called repeatedly by the arduino processor */
void loop()
{
  //Process any ACI commands or events from the NRF8001- main BLE handler, must run often. Keep main loop short.
  aci_loop();
 
  //Check if data is available
  if (ble_rx_buffer_len)
  {
    uint16_t pos;
    sscanf((char*)ble_rx_buffer, "%d", &pos);
   
    if(oldPos == -1)
    {
      moveAllServos(lower, pos);     
    }
    else
    {
      moveAllServos(oldPos, pos);     
    }   
   
    SerialMonitorInterface.println(pos);

    oldPos = pos;
    ble_rx_buffer_len = 0;//clear afer reading
  }
}

/* This method is a delegate method to the BLEsetup function included in the STBLE.zip drivers.
   This is for readability during the setup phase. */
void loadBluetoothDrivers()
{
   BLEsetup();
}

/* This method loads the drivers for the servo controller shield for communication with up to 4 servo motors */
void loadServoDrivers()
{
  Wire.begin();
  pinMode(9, OUTPUT); //Pin 9 is the reset pin for the servo controller TinyShield
 
  // THE FOLLOWING 2 LINES MUST BE COMMENTED OUT FOR COMPATIBILITY WITH BLUETOOTH SHIELD
  // BECAUSE DROPPING THE POWER LOW ON PIN 9 RESETS THE BLUETOOTH AND IT WILL NOT RECOVER
  // digitalWrite(9, LOW);
  // delay(10);
 
  digitalWrite(9, HIGH);
  delay(100);
 
  //Set the period to 20000us or 20ms, correct for driving most servos
  if(servoDriver.begin(20000) != 0)
  {
    SerialMonitorInterface.println("Motor driver not detected!");
    while(1);
  }
}

/* This method is a convienience method to set all servo motors to the same position */
void moveAllServos(int from, int to)
{
  servoDriver.setServo(1, to);
  servoDriver.setServo(2, to);
  servoDriver.setServo(3, to);
  servoDriver.setServo(4, to);
 
  /* Time it takes servo to move to specified position based on servo speed rating at 60 deg turn */
  long delayMS = long(1000.0 * speed * abs(from-to));
  SerialMonitorInterface.print("Delay ");
  SerialMonitorInterface.print(delayMS);
  SerialMonitorInterface.println(" ms");
  delay(delayMS);
}
File UART.ino taken directly from UARTPassthrough example project (changed BLE broadcast name only):

Code: [Select]

#if BLE_DEBUG
#include <stdio.h>
char sprintbuff[100];
#define PRINTF(...) {sprintf(sprintbuff,__VA_ARGS__);SerialMonitorInterface.print(sprintbuff);}
#else
#define PRINTF(...)
#endif


volatile uint8_t set_connectable = 1;
uint16_t connection_handle = 0;


#define  ADV_INTERVAL_MIN_MS  50
#define  ADV_INTERVAL_MAX_MS  100


int connected = FALSE;


int BLEsetup() {
  int ret;

  HCI_Init();
  /* Init SPI interface */
  BNRG_SPI_Init();
  /* Reset BlueNRG/BlueNRG-MS SPI interface */
  BlueNRG_RST();

  uint8_t bdaddr[] = {0x12, 0x34, 0x00, 0xE1, 0x80, 0x02};

  ret = aci_hal_write_config_data(CONFIG_DATA_PUBADDR_OFFSET, CONFIG_DATA_PUBADDR_LEN, bdaddr);

  if (ret) {
    PRINTF("Setting BD_ADDR failed.\n");
  }

  ret = aci_gatt_init();

  if (ret) {
    PRINTF("GATT_Init failed.\n");
  }

  uint16_t service_handle, dev_name_char_handle, appearance_char_handle;
  ret = aci_gap_init_IDB05A1(GAP_PERIPHERAL_ROLE_IDB05A1, 0, 0x07, &service_handle, &dev_name_char_handle, &appearance_char_handle);

  if (ret) {
    PRINTF("GAP_Init failed.\n");
  }

  const char *name = "BlueNRG";

  ret = aci_gatt_update_char_value(service_handle, dev_name_char_handle, 0, strlen(name), (uint8_t *)name);

  if (ret) {
    PRINTF("aci_gatt_update_char_value failed.\n");
  } else {
    PRINTF("BLE Stack Initialized.\n");
  }

  ret = Add_UART_Service();

  if (ret == BLE_STATUS_SUCCESS) {
    PRINTF("UART service added successfully.\n");
  } else {
    PRINTF("Error while adding UART service.\n");
  }

  /* +4 dBm output power */
  ret = aci_hal_set_tx_power_level(1, 3);
}

void aci_loop() {
  HCI_Process();
  ble_connection_state = connected;
  if (set_connectable) {
    setConnectable();
    set_connectable = 0;
  }
  if (HCI_Queue_Empty()) {
    //Enter_LP_Sleep_Mode();
  }
}

#define COPY_UUID_128(uuid_struct, uuid_15, uuid_14, uuid_13, uuid_12, uuid_11, uuid_10, uuid_9, uuid_8, uuid_7, uuid_6, uuid_5, uuid_4, uuid_3, uuid_2, uuid_1, uuid_0) \
  do {\
    uuid_struct[0] = uuid_0; uuid_struct[1] = uuid_1; uuid_struct[2] = uuid_2; uuid_struct[3] = uuid_3; \
    uuid_struct[4] = uuid_4; uuid_struct[5] = uuid_5; uuid_struct[6] = uuid_6; uuid_struct[7] = uuid_7; \
    uuid_struct[8] = uuid_8; uuid_struct[9] = uuid_9; uuid_struct[10] = uuid_10; uuid_struct[11] = uuid_11; \
    uuid_struct[12] = uuid_12; uuid_struct[13] = uuid_13; uuid_struct[14] = uuid_14; uuid_struct[15] = uuid_15; \
  }while(0)

#define COPY_UART_SERVICE_UUID(uuid_struct)  COPY_UUID_128(uuid_struct,0x6E, 0x40, 0x00, 0x01, 0xB5, 0xA3, 0xF3, 0x93, 0xE0, 0xA9, 0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E)
#define COPY_UART_TX_CHAR_UUID(uuid_struct)  COPY_UUID_128(uuid_struct,0x6E, 0x40, 0x00, 0x02, 0xB5, 0xA3, 0xF3, 0x93, 0xE0, 0xA9, 0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E)
#define COPY_UART_RX_CHAR_UUID(uuid_struct)  COPY_UUID_128(uuid_struct,0x6E, 0x40, 0x00, 0x03, 0xB5, 0xA3, 0xF3, 0x93, 0xE0, 0xA9, 0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E)

uint16_t UARTServHandle, UARTTXCharHandle, UARTRXCharHandle;


uint8_t Add_UART_Service(void)
{
  tBleStatus ret;
  uint8_t uuid[16];

  COPY_UART_SERVICE_UUID(uuid);
  ret = aci_gatt_add_serv(UUID_TYPE_128,  uuid, PRIMARY_SERVICE, 7, &UARTServHandle);
  if (ret != BLE_STATUS_SUCCESS) goto fail;

  COPY_UART_TX_CHAR_UUID(uuid);
  ret =  aci_gatt_add_char(UARTServHandle, UUID_TYPE_128, uuid, 20, CHAR_PROP_WRITE_WITHOUT_RESP, ATTR_PERMISSION_NONE, GATT_NOTIFY_ATTRIBUTE_WRITE,
                           16, 1, &UARTTXCharHandle);
  if (ret != BLE_STATUS_SUCCESS) goto fail;

  COPY_UART_RX_CHAR_UUID(uuid);
  ret =  aci_gatt_add_char(UARTServHandle, UUID_TYPE_128, uuid, 20, CHAR_PROP_NOTIFY, ATTR_PERMISSION_NONE, 0,
                           16, 1, &UARTRXCharHandle);
  if (ret != BLE_STATUS_SUCCESS) goto fail;

  return BLE_STATUS_SUCCESS;

fail:
  PRINTF("Error while adding UART service.\n");
  return BLE_STATUS_ERROR ;

}



uint8_t lib_aci_send_data(uint8_t ignore, uint8_t* sendBuffer, uint8_t sendLength) {
  return !Write_UART_TX((char*)sendBuffer, sendLength);
}

uint8_t Write_UART_TX(char* TXdata, uint8_t datasize)
{
  tBleStatus ret;

  ret = aci_gatt_update_char_value(UARTServHandle, UARTRXCharHandle, 0, datasize, (uint8_t *)TXdata);

  if (ret != BLE_STATUS_SUCCESS) {
    PRINTF("Error while updating UART characteristic.\n") ;
    return BLE_STATUS_ERROR ;
  }
  return BLE_STATUS_SUCCESS;

}


void Read_Request_CB(uint16_t handle)
{
  /*if(handle == UARTTXCharHandle + 1)
    {

    }
    else if(handle == UARTRXCharHandle + 1)
    {


    }*/

  if (connection_handle != 0)
    aci_gatt_allow_read(connection_handle);
}


void setConnectable(void)
{
  tBleStatus ret;

  // const char local_name[] = {AD_TYPE_COMPLETE_LOCAL_NAME, 'B', 'l', 'u', 'e', 'N', 'R', 'G'};
  const char local_name[] = {AD_TYPE_COMPLETE_LOCAL_NAME, 'D', 'u' ,'n' ,'c' ,'a' ,'n'};

  hci_le_set_scan_resp_data(0, NULL);
  PRINTF("General Discoverable Mode.\n");

  ret = aci_gap_set_discoverable(ADV_IND,
                                 (ADV_INTERVAL_MIN_MS * 1000) / 625, (ADV_INTERVAL_MAX_MS * 1000) / 625,
                                 STATIC_RANDOM_ADDR, NO_WHITE_LIST_USE,
                                 sizeof(local_name), local_name, 0, NULL, 0, 0);

  if (ret != BLE_STATUS_SUCCESS)
    PRINTF("%d\n", (uint8_t)ret);

}

void Attribute_Modified_CB(uint16_t handle, uint8_t data_length, uint8_t *att_data)
{
  if (handle == UARTTXCharHandle + 1) {
    int i;
    for (i = 0; i < data_length; i++) {
      ble_rx_buffer[i] = att_data[i];
    }
    ble_rx_buffer[i] = '\0';
    ble_rx_buffer_len = data_length;
  }
}

void GAP_ConnectionComplete_CB(uint8_t addr[6], uint16_t handle) {

  connected = TRUE;
  connection_handle = handle;

  PRINTF("Connected to device:");
  for (int i = 5; i > 0; i--) {
    PRINTF("%02X-", addr[i]);
  }
  PRINTF("%02X\r\n", addr[0]);
}

void GAP_DisconnectionComplete_CB(void) {
  connected = FALSE;
  PRINTF("Disconnected\n");
  /* Make the device connectable again. */
  set_connectable = TRUE;
}



void HCI_Event_CB(void *pckt)
{
  hci_uart_pckt *hci_pckt = (hci_uart_pckt *)pckt;
  hci_event_pckt *event_pckt = (hci_event_pckt*)hci_pckt->data;

  if (hci_pckt->type != HCI_EVENT_PKT)
    return;

  switch (event_pckt->evt) {

    case EVT_DISCONN_COMPLETE:
      {
        //evt_disconn_complete *evt = (void *)event_pckt->data;
        GAP_DisconnectionComplete_CB();
      }
      break;

    case EVT_LE_META_EVENT:
      {
        evt_le_meta_event *evt = (evt_le_meta_event *)event_pckt->data;

        switch (evt->subevent) {
          case EVT_LE_CONN_COMPLETE:
            {
              evt_le_connection_complete *cc = (evt_le_connection_complete *)evt->data;
              GAP_ConnectionComplete_CB(cc->peer_bdaddr, cc->handle);
            }
            break;
        }
      }
      break;

    case EVT_VENDOR:
      {
        evt_blue_aci *blue_evt = (evt_blue_aci *)event_pckt->data;
        switch (blue_evt->ecode) {

          case EVT_BLUE_GATT_READ_PERMIT_REQ:
            {
              evt_gatt_read_permit_req *pr = (evt_gatt_read_permit_req *)blue_evt->data;
              Read_Request_CB(pr->attr_handle);
            }
            break;

          case EVT_BLUE_GATT_ATTRIBUTE_MODIFIED:
            {
              evt_gatt_attr_modified_IDB05A1 *evt = (evt_gatt_attr_modified_IDB05A1*)blue_evt->data;
              Attribute_Modified_CB(evt->attr_handle, evt->data_length, evt->att_data);
            }
            break;
        }
      }
      break;
  }

}
I am simply connecting to the BLE shield via an Android app similar to nRF UART 2.0 (nordic semi) and sending servo pulse micro second values to the BLE shield as a string, once its received in the Arduino sketch, I convert it back to an integer and write that value to the servos. The minimum sized message I send to the BLE shield from the app would be a string "510" and the max value sent would be the string "2540". I have implemented a virtual joystick in the android app I am using and if i send pulse updates to the sketch too fast the BLE comms end up freezing the whole app into a non-recoverable state. If I lower the send interval to around 10 times/sec I don't experience sketch freezes as frequently but it is still and issue. I'd like to understand the root cause of this issue so I can implement an applicable solution where I can get the highest frequency servo pulse updates possible. I want to have smooth servo operation with little to no jittering. The lower the frequency I am restricted to the more jittering I experience in the servos for smaller pulse movements.

Please advise.

Thank you.

[Ben Rose]

Looking through the code, I do see a delay being used in 'moveAllServos'- hard to say exactly how long the delay is(it looks like it's 0 if the servos don't change position but may be 10s or 100s of ms on large changes), but any blocking delay like that should be avoided to keep the main loop running frequently, especially variable delays. Let us know if removing that changes what you are seeing.

Thanks,
Ben

[jakehenry41kc]

Hi Ben,

Thanks for the support on the issues I've been having I really appreciate it.

I removed the delay and the problem did not go away.
Even if I remove the call to moveAllServos from within the loop() the sketch freezes if I send bluetooth too fast. I also tried without loading the servo drivers in the code as well.
The sketch freezes even if I merely read the bluetooth data and print it to SerialMonitorInterface with no servo operation whatsoever. This is a turning out to be a pretty large hurdle for the project I am on. I tried batching multiple pulse values into a single string message, sending at a lower freq, and parsing them out in sequence on the Arduino side in hopes that I could do it that way, but to no avail. I can't seem to get around the need for high frequency so that I can get smooth servo movement at mid-to-low speeds. As it stands sending at a rate higher than 50ms results in rather jittery servo operation.

Do you have any other suggestions? The problem appears to be isolated to Bluetooth comms with the BLE ST shield. There is no correlation to anything regarding when the comms drop out. Its intermittent and occurs at random periods of time. I am supplying a steady 5.2V of current to the shield stack at all times even under load from the servos.
I am not sure if you have a test setup at TinyCircuits or not but this problem is pretty easily reproducible, atleast on my set of shields, if you want to observe for yourself.

Look forward to hearing from you.

Thanks,
Jake

[jakehenry41kc]

I ran another test just now using the BLEServoExample.ino project that was recently added to the blogs section of TinyCircuits website. I got the same problem when running that example code as well as with JUST the UARTPassthrough example.

An important thing to note is that the main sketch loop continues to run but the call to aciloop() is either throwing a non-catchable error is not detecting input if Bluetooth is sent too fast. For examples sake I am sending a max of a 4 byte integer every 16ms for a total max of 240 bytes per second in the worst case scenario. The BLE code will run for a short (random) time period but eventually stop detecting input.

So basically:

• the main loop is running verfied via a SerialPrint on every loop after the error state occurs
• call to aciloop is not changing the value of ble_rx_buffer_length variable to non-zero
• most likely not filling the ble_rx_buffer either

My suspicion is that there is some kind of error occurring deep inside the UART.ino portion of the project that does not cause the main loop to stop, but causes all future RX_Characteristic writes from an external bluetooth server to be ignored. I am not really sure where to go from here.