Attitude monitor using Peripheral:XIAO_BLE_Sence and Central:XIAO_BLE with mbed 2.7.2 and ArduinoBLE

For my BLE programming practice, I made a project to send attitude data from a battery-powered peripheral:XIAO_BLE_Sense to a central:XIAO_BLE for monitering.The board library is mbed 2.7.2.
It was difficult to find a sample that could be used directly, but the example in the following link was used as a reference.

I used the following libraries.

ArduinoBLE 1.3.2 by Arduino
Seeed Arduino LSM6DS3 2.0.3 by Seeed Studio
Madgwick 1.2.0 by Arduino
Adafruit GFX Library 1.10.14 by Adafruit
Adafruit SSD1306 2.5.3 by Adafruit

So far, it is working as expected with no problems, but this is my first time using BLE and I am not sure if my code is appropriate. If there is anything wrong with BLE code, please advise.

XIAO_BLE_mbed_Attitude_Monitor.zip (5.5 KB)

Peripheral XIAO BLE Sence

//----------------------------------------------------------------------------------------------
// Board Library : Seeed nRF52 mbed-enabled Borads 2.7.2
// Board Select  : Seeed nRF52 mbed-enabled Borads / Seeed XIAO BLE Sense - nRF52840
// 2022/10/07
//----------------------------------------------------------------------------------------------
// Modification required for battery voltage reading 
// /Arduino15/packages/Seeeduino/hardware/mbed/2.7.2/cores/arduino/pinDefinitions.h:22
// #define analogPinToPinName(P)
//    g_APinDescription ---> g_AAnalogPinDescription
// https://forum.seeedstudio.com/t/xiao-ble-sense-mbed-2-7-2-battery-charge-and-voltage-monitor-analogread-p0-31-does-not-work/266438
//**********************************************************************************************

#include <Wire.h>
#include <ArduinoBLE.h>     // ArduinoBLE 1.3.2 by Arduino       
#include <MadgwickAHRS.h>   // Madgwick 1.2.0 by Arduino
#include <LSM6DS3.h>        // Seeed Arduino LSM6DS3 2.0.3  by Seeed Studio

#define AVENUM      32      // average number of ADC
#define HICHG       P0_13   // charge current setting pin Open:50mA Lo:100mA
#define CHG         P0_17   // charge indicator pin Open:discharge Lo:charge(LED ON)
#define VBAT_ENABLE P0_14   // battery vontage read enable Open:disable Lo:enable
#define VBAT_READ   P0_31   // battery voltage monitor pin
#define VBAT_LOWER  3.5     // battery voltage lower limit
#define VBAT_UPPER  4.2     // battery voltage upper limit
#define dataNum 8           //send data number : rolll, pitch, yaw, Vbatt (4 data 8 byte)

LSM6DS3 myIMU(I2C_MODE, 0x6A); // IMU
Madgwick  filter;              // Madgwick filter

const char* versionNumber = "0.90"; // version number
float ax, ay, az;                   // Accel
float gx, gy, gz;                   // Gyro
float Vbatt;                        // battery voltage
float roll, pitch, yaw;             // attitude
uint8_t readData;                   // for reading IMU register
uint32_t timestamp;                 // for delay function
bool LED_state;                     // LED ON/OFF state
union unionData {                   //Union for bit convertion 16 <--> 8
  int16_t   dataBuff16[dataNum/2];
  uint8_t   dataBuff8[dataNum];
};
union unionData ud;

//generated Linux command uuidgen "0dd7eb5a-a8b9-4f45-bcd7-94c674c3b25f"
#define myUUID(val) ("0dd7eb5a-" val "-4f45-bcd7-94c674c3b25f")
BLEService        AttService(myUUID("0000"));
BLECharacteristic dataCharacteristic(myUUID("0010"), BLERead | BLENotify, dataNum);

void setup() {
  //initialize serial port
  Serial.begin(115200);
//  while (!Serial) { delay(1); }

  //set I/O pins
  pinMode(LED_RED, OUTPUT);       // LOW:LED ON, HIGH:LED OFF
  pinMode(LED_GREEN, OUTPUT);
  pinMode(HICHG, OUTPUT);
  pinMode(CHG, INPUT);
  pinMode(VBAT_ENABLE, OUTPUT);
  pinMode(VBAT_READ, INPUT);
  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_GREEN, HIGH);
  digitalWrite(HICHG, LOW);         // charge current 100mA
  digitalWrite(VBAT_ENABLE, LOW);   // battery voltage read enable

  // initialize ADC 2.4V/4096
  analogReference(AR_INTERNAL2V4);  // Vref=2.4V
  analogAcquisitionTime(AT_10_US);
  analogReadResolution(12);         // 4096

  // initialize and set IMU
  // refer to   LSM6D3.cpp:351
  myIMU.settings.gyroRange = 2000;  // calcGyro()
  myIMU.settings.accelRange = 4;    // calcAccel()
  if (myIMU.begin() != 0) {                                      
    Serial.println("IMU Device error");
    while(1);
  }
  Wire1.setClock(400000UL);  //SCL 400kHz
    
  // change defalt settings, refer to data sheet 9.13, 9.14, 9.19, 9.20
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, 0x1C);    // 12.5Hz 2000dps
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x1A);   // 12.5Hz 4G 
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL7_G, 0x00);    // HPF 16mHz
  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL8_XL, 0x09);   // ODR/4

  // Maadgwick filter sampling rate
  filter.begin(12.5);  

  // initialize BLE
  if (!BLE.begin()) {
    Serial.println("starting BLE module failed!");
    while (1);
  }

  // set the local name
  BLE.setLocalName("Att_Monitor");
  // set the device name
  BLE.setDeviceName("XIAO nRF52840 Sence");
  // set the UUID for the service
  BLE.setAdvertisedService(AttService);
  // add the characteristic to the service  
  AttService.addCharacteristic(dataCharacteristic);
  // add service  
  BLE.addService(AttService);

  // start advertising
  BLE.setAdvertisingInterval(160);    //0.625mS * 160 = 100mS
  BLE.setConnectionInterval(6, 3200); //1.25mS * 6 = 7.5mS, 1.25mS * 3200 = 4S
  BLE.advertise();

}

void loop() {
  // connect the sentral
  Serial.println("Connecting to Central ........");
  BLEDevice central = BLE.central();

  if (central) {    // connected to Central? 
  
    // while connected Central
    Serial.println("Connected Central");
    while(central.connect()) {      
      LED_state = !LED_state;
      digitalWrite(LED_GREEN, (LED_state ? LOW : HIGH));   // connect indicator blinking

      // wait for IMU data to become valid
      // sample rate is 12.5Hz, so can read every 80mS    
      do {        
        myIMU.readRegister(&readData, LSM6DS3_ACC_GYRO_STATUS_REG);   //0,0,0,0,0,TDA,GDA,XLDA
      } while ((readData & 0x07) != 0x07);        

      digitalWrite(LED_RED, LOW);   // data read and send task indicator ON
               
      ax = myIMU.readFloatAccelX(); // Accel data
      ay = myIMU.readFloatAccelY();
      az = myIMU.readFloatAccelZ();
      gx = myIMU.readFloatGyroX();  // Gyro data
      gy = myIMU.readFloatGyroY();
      gz = myIMU.readFloatGyroZ();

      // calculate the attitude with Madgwick filter
      filter.updateIMU(gx, gy, gz, ax, ay, az);

      roll = filter.getRoll();    // -180 ~ 180deg
      pitch = filter.getPitch();  // -180 ~ 180deg
      yaw = filter.getYaw();      // 0 -3 60deg

      // battery voltage averasing  32 averaging 1 ~ 6mS
      int Vadc = 0;
      for (int i = 0; i < AVENUM; i++) {        
        Vadc = Vadc + analogRead(VBAT_READ);    // analogRead() 32uS       
      }
      
      Vadc = Vadc / AVENUM;
      Vbatt = 2.961 * 2.4 * Vadc / 4096 * 1.0196;     // Vref=2.4, 1/attination=(510e3 + 1e6)/510e3=2.961
                                                      // correction=1.0196(option)
      // convert float data to uint16_t data
      ud.dataBuff16[0] = (roll * 32768) / 180; 
      ud.dataBuff16[1] = (pitch * 32768) / 180; 
      ud.dataBuff16[2] = (yaw * 32768) / 360;       
      ud.dataBuff16[3] = Vbatt * 1000;
     
      // for Serial plotter 5~20mS
      Serial.print(yaw);
      Serial.print(" ");
      Serial.print(pitch);
      Serial.print(" ");
      Serial.print(roll);
      Serial.println();

      // write to Characteristic as byte data        
      dataCharacteristic.writeValue(ud.dataBuff8, dataNum);   // 40~100uS
       
      digitalWrite(LED_RED, HIGH);  // data read and send task indicator OFF
      
    } //while connected
      
    // if disconnected from Central
    digitalWrite(LED_GREEN, HIGH);                // connect indicator OFF
    Serial.print("Disconnected from central: ");
    Serial.println(central.address());
  } //if connect
    
} //loop()

Central XIAO BLE

//----------------------------------------------------------------------------------------------
//Board Library : Seeed nRF52 mbed-enable Borads 2.7.2
//Board Select  : Seeed nRF52 mbed-enable Borads / Seeed XIAO BLE - nRF52840
//2022/10/08
//----------------------------------------------------------------------------------------------

#include <Wire.h>
#include <Adafruit_GFX.h>         // Adafruit GFX Library 1.10.14 by Adafruit
#include "Fonts/FreeSans9pt7b.h"
#include "Fonts/FreeSerif9pt7b.h"
#include <Adafruit_SSD1306.h>     // Adafruit SSD1306 2.5.3 by Adafruit
#include <ArduinoBLE.h>           // ArduinoBLE 1.3.2 by Arduino

#define VBAT_LOWER  3.5           // battery voltage lower limit
#define VBAT_UPPER  4.2           // battery voltage upper limit
#define dataNum 8                 // receive data number : roll, pitch, yaw, Vbatt (4 data 8 byte)

Adafruit_SSD1306 display(128, 64, &Wire, -1);   //SSD1306

const char* versionNumber = "0.90"; // version number
float roll, pitch, yaw;             // attitude
float Vbatt;                        // battery voltage
int ss;                             // peripheral signal strength
bool readingFlag = false;           // data buffer in use flag
int err;                            // error code of scan_connect function
bool LED_state;                     // LED ON/OFF state 
union unionData {                   // Union for bit convertion 16 <--> 8
  int16_t   dataBuff16[dataNum/2];
  uint8_t   dataBuff8[dataNum];
};
union unionData ud;

// Characteristic UUID
#define myUUID(val) ("0dd7eb5a-" val "-4f45-bcd7-94c674c3b25f")
//BLEService        AttService(myUUID("0000"));
BLECharacteristic dataCharacteristic(myUUID("0010"), BLEWrite | BLENotify, dataNum);

BLEDevice peripheral;

void setup()
{
  //initialize serial port
  Serial.begin(115200);
  while (!Serial) { delay(1); }

  //set I/O pins
  pinMode(LED_RED, OUTPUT);       // LOW:LED ON, HIGH:LED OFF
  pinMode(LED_GREEN, OUTPUT);
  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_GREEN, HIGH);

  // initialize SSD1306
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 initialize error");
  }
  display.clearDisplay();
  display.setFont(&FreeSerif9pt7b);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 15);
  display.println("ATT Monitor");
  display.setCursor(0, 31);
  display.println("     BLE Central");
  display.setCursor(0, 47);
  display.print("Ver   : ");
  display.println(versionNumber);
  display.display();
  delay(2000);

  // initialize BLE
  BLE.begin(); 
}

//******************************************************************************************
// IMU data is received every 80mS
//
void loop() {
  // scan and connect the peripheral
  // return value of this function is an error code 
  err = scan_connect();
  
  // if connected? 
  if(err == 0) {            
    // main task while connected to the peripheral 60~78mS
    while (peripheral.connected()) {     
      LED_state = !LED_state;
      digitalWrite(LED_GREEN, (LED_state ? LOW : HIGH));   // connect indicator blinking
      
      long timestamp = millis();    // for timer
      
      // restoration of received data
      // prohibit event handler access while data buffer is in use          
      readingFlag = true;   
        roll = ud.dataBuff16[0] * 180.0 / 32768.0;
        pitch = ud.dataBuff16[1] * 180.0 / 32768.0;
        yaw = ud.dataBuff16[2] * 360.0 / 32768.0;
        Vbatt = ud.dataBuff16[3] / 1000.0;
      readingFlag = false;

      // for serial plotter
      Serial.print(roll); Serial.print(", ");
      Serial.print(pitch); Serial.print(", ");
      Serial.println(yaw);
   
      // display attitude, battery voltage, signal strength
      display.clearDisplay();      
      display.setCursor(0, 15);
      display.print("Roll   "); display.println(roll);
      display.setCursor(0, 31);
      display.print("Pitch  "); display.print(pitch);
      display.setCursor(0, 47);
      display.print("Yaw  "); display.println(yaw);    
      display.setCursor(0, 63);
      display.print("Vbatt "); display.print(Vbatt);
      if(Vbatt < VBAT_LOWER) {
        display.print("L");
      }
      display.setCursor(96, 63);
      display.println(ss);      
      display.display();

      while(millis() - timestamp < 80);     // wait for loop time 80mS
    } //While connected
      
  } //if scan & connect
    
  // if disconnected from the peripheral
  digitalWrite(LED_GREEN, HIGH);            // connect indicator OFF
  Serial.print("Disconnected from the peripheral: ");
  Serial.println(peripheral.address());
  Serial.print("ERROR : ");                 // return value of scan_connect
  Serial.println(err);
      
} //loop

//**************************************************************************************************
// scan and connect the peripheral
// return value of this function is an error code
//
int scan_connect(void) {
  // scanning peripherals
  BLE.scanForUuid(myUUID("0000"));  
  Serial.println("1.SCANNING ................");

  peripheral = BLE.available();
  
  if (!peripheral) {
    Serial.println("2x.Peripheral unavailable");
    return 2;
  }
  Serial.println("2.Peripheral is available");
    
  if (peripheral.localName() != "Att_Monitor") {
    Serial.println("3x.Peripheral local name miss match");
    return 3;
  }
  Serial.println("3.Got the right peripheral");

  // stop scanning, connect the peripheral
  BLE.stopScan();
  Serial.println("4.Stop scanning");
  
  Serial.println("5.CONNECTING ................");
  if (!peripheral.connect()) {
    Serial.println("5x.Can't connect");
    return 5;
  } 
  Serial.println("5.Connected");
  
  if (!peripheral.discoverAttributes()) {
    Serial.println("6x.Didn't discover attributes");
    peripheral.disconnect();
    return 6;
  }
  Serial.println("6.Discovered attributes");

  dataCharacteristic = peripheral.characteristic(myUUID("0010"));         //dataCaracteristic UUID
  dataCharacteristic.setEventHandler(BLEWritten, characteristicWritten);  //BLEWritten handler
  Serial.println("7.Char and eventhandler setting");  

  if (!dataCharacteristic.canSubscribe()) {
    Serial.println("8x.Can't subscribe");
    peripheral.disconnect();
    return 8;
  }
  Serial.println("8.Can subscribe");

  if (!dataCharacteristic.subscribe()) {
    Serial.println("9x.Can't Subscribe");
    peripheral.disconnect();
    return 9;
  }
  Serial.println("9.Subscribed");

  Serial.println("10.Success scanning and connecting");
  return 0;
}

//****************************************************************************************************
// Characteristic written event handler
//
void characteristicWritten(BLEDevice peripheral, BLECharacteristic thisChar) {

  digitalWrite(LED_RED, LOW);         // event indicator ON
    
  // wait while data buffer is accessed in main loop  
  while(readingFlag == true) {
  }    
  dataCharacteristic.readValue(ud.dataBuff8, dataNum);  // read data packet 3.5uS  
  ss = peripheral.rssi();                               // read signal strength 5~20mS
    
  long timestamp = millis();
  while(millis() - timestamp <= 1);   // Delay to make LED visible
    
  digitalWrite(LED_RED, HIGH);        // event indicator OFF
}

1 Like

:tada: :tada: Good~ 使用了如期运行。感谢!
接下来想进行低功耗工作设计。

I’ll just say this is “AWESOME” I’ve been taught all code is garbage! LOL My code that is :laughing:

This is some of the best code I’ve seen on this Forum. Clean and well documented. Seeed Should hire you to re-write their examples. Thank you very, very much for sharing. So many of us just need that little push in the right direction and to learn a little style and good technique. I have struggled with the battery voltage methods and all of my outcomes have been unreliable. I also need to get the IMU data on my BLE app, but only to detect or sense motion or if the item has been moved, picked up, or dropped. I love the journey but my eyes are tired and red. LOL :star_struck:

Thanks again and please continue to share this very interesting project and Your approach to solving the challenges that it and Seeed Studio present … :stuck_out_tongue: :ok_hand:

GL :-p