I am trying to make a BLE service that provides 3 characteristics from the attached accelerometer (pitch, roll, yaw) - in the attached code, I implemented Yaw and Pitch only. My problem is that when I try to advertise, the "TX characteristics" outputs good values but the custom one (Pitch) outputs data in HEX (I suppose) and afterwards the real data in the quotation marks.
This is a sketch I put together:
(Check also an output of the nRF Connect App (attached SS))
Code: Select all
/*
Video: https://www.youtube.com/watch?v=oCMOYS71NIU
Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleNotify.cpp
Ported to Arduino ESP32 by Evandro Copercini
Create a BLE server that, once we receive a connection, will send periodic notifications.
The service advertises itself as: 6E400001-B5A3-F393-E0A9-E50E24DCCA9E
Has a characteristic of: 6E400002-B5A3-F393-E0A9-E50E24DCCA9E - used for receiving data with "WRITE"
Has a characteristic of: 6E400003-B5A3-F393-E0A9-E50E24DCCA9E - used to send data with "NOTIFY"
The design of creating the BLE server is:
1. Create a BLE Server
2. Create a BLE Service
3. Create a BLE Characteristic on the Service
4. Create a BLE Descriptor on the characteristic
5. Start the service.
6. Start advertising.
In this example rxValue is the data received (only accessible inside that function).
And txValue is the data to be sent, in this example just a byte incremented every second.
*/
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include "MPU9250.h"
BLECharacteristic *pCharacteristic;
BLECharacteristic *pCharacteristic1;
bool deviceConnected = false;
float txValueYaw = 0;
float txValuePitch = 0;
const int readPin = 32; // Use GPIO number. See ESP32 board pinouts
const int LED = 2; // Could be different depending on the dev board. I used the DOIT ESP32 dev board.
double roll , pitch, yaw;
MPU9250 IMU(Wire,0x68);
int status;
//std::string rxValue; // Could also make this a global var to access it in loop()
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX1 "6E400004-B5A3-F393-E0A9-E50E24DCCA9E"
class MyServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
};
void onDisconnect(BLEServer* pServer) {
deviceConnected = false;
}
};
class MyCallbacks: public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic *pCharacteristic) {
std::string rxValue = pCharacteristic->getValue();
if (rxValue.length() > 0) {
Serial.println("*********");
Serial.print("Received Value: ");
for (int i = 0; i < rxValue.length(); i++) {
Serial.print(rxValue[i]);
}
Serial.println();
// Do stuff based on the command received from the app
if (rxValue.find("A") != -1) {
Serial.print("Turning ON!");
digitalWrite(LED, HIGH);
}
else if (rxValue.find("B") != -1) {
Serial.print("Turning OFF!");
digitalWrite(LED, LOW);
}
Serial.println();
Serial.println("*********");
}
}
};
void setup() {
Serial.begin(115200);
while(!Serial){}
setIMUtoRead();
setCalibrationIMU();
pinMode(LED, OUTPUT);
// Create the BLE Device
BLEDevice::init("ESP32 UART Test"); // Give it a name
// Create the BLE Server
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
// Create the BLE Service
BLEService *pService = pServer->createService(SERVICE_UUID);
// Create a BLE Characteristic
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_TX,
BLECharacteristic::PROPERTY_NOTIFY
);
pCharacteristic->addDescriptor(new BLE2902());
pCharacteristic1 = pService->createCharacteristic(
CHARACTERISTIC_UUID_TX1,
BLECharacteristic::PROPERTY_NOTIFY
);
pCharacteristic1->addDescriptor(new BLE2902());
BLECharacteristic *pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_RX,
BLECharacteristic::PROPERTY_WRITE
);
pCharacteristic->setCallbacks(new MyCallbacks());
// Start the service
pService->start();
// Start advertising
pServer->getAdvertising()->start();
Serial.println("Waiting a client connection to notify...");
}
void loop() {
if (deviceConnected) {
IMU.readSensor();
float accelX = IMU.getAccelX_mss();
float accelY = IMU.getAccelY_mss();
float accelZ = IMU.getAccelZ_mss();
float gyroX = IMU.getGyroX_rads()/57.3;
float gyroY = IMU.getGyroY_rads()/57.3;
float gyroZ = IMU.getGyroZ_rads()/57.3;
float magX = IMU.getMagX_uT();
float magY = IMU.getMagY_uT();
float magZ = IMU.getMagZ_uT();
pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ))));
roll = atan2(-accelX ,accelZ);
// yaw from mag
float Yh = (magY * cos(roll)) - (magZ * sin(roll));
float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch));
yaw = atan2(Yh, Xh);
roll = roll*57.3;
pitch = pitch*57.3;
yaw = yaw*57.3; // in degrees /Angle relative to north
if (yaw < 0.1){
yaw = yaw + 360; // in degrees /relative to north
}
// Fabricate some arbitrary junk for now...
txValueYaw = yaw;// This could be an actual sensor reading!
txValuePitch = pitch;
// Let's convert the value to a char array:
char txStringYaw[8]; // make sure this is big enuffz
char txStringPitch[8];
dtostrf(txValueYaw, 1, 2, txStringYaw); // float_val, min_width, digits_after_decimal, char_buffer
dtostrf(txValuePitch, 1, 2, txStringPitch);
// pCharacteristic->setValue(&txValue, 1); // To send the integer value
// pCharacteristic->setValue("Hello!"); // Sending a test message
pCharacteristic->setValue(txStringYaw);
pCharacteristic1->setValue(txStringPitch);
pCharacteristic->notify(); // Send the value to the app!
pCharacteristic1->notify();
Serial.print("Yaw: ");
Serial.print(txStringYaw);
Serial.print(", Pitch: ");
Serial.print(txStringPitch);
Serial.println("***********");
// You can add the rxValue checks down here instead
// if you set "rxValue" as a global var at the top!
// Note you will have to delete "std::string" declaration
// of "rxValue" in the callback function.
// if (rxValue.find("A") != -1) {
// Serial.println("Turning ON!");
// digitalWrite(LED, HIGH);
// }
// else if (rxValue.find("B") != -1) {
// Serial.println("Turning OFF!");
// digitalWrite(LED, LOW);
// }
}
delay(50);
}
void setIMUtoRead(){
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check wiring of your circuitiry.");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
IMU.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_10HZ);
IMU.setSrd(25);
//Calibration of MPU
Serial.println("Status: set stuff");
Serial.println("Status: calibrate mag");
Serial.println("Status: tail 8");
status = IMU.calibrateMag();
Serial.println("Status: Done");
Serial.println("Status: calibrate Accel ");
status = IMU.calibrateAccel();
Serial.println("Status: calibratie Gyro");
status = IMU.calibrateGyro();
Serial.println("Status: done");
//Setting Values
}
void setCalibrationIMU(){
float hxb = 9.73;
float hxs = 1.02;
float hyb = 0.81;
float hys = 0.96;
float hzb = -19.53;
float hzs = 1.03;
float azb = 0.00;
float azs = 1.00;
float ayb = 0.00;
float ays = 1.00;
float axb = 0.00;
float axs = 0.00;
float gzb = 0.03;
float gyb = 0.01;
float gxb = -0.01;
IMU.setGyroBiasX_rads(gxb);
IMU.setGyroBiasY_rads(gyb);
IMU.setGyroBiasZ_rads(gzb);
IMU.setAccelCalX(axb,axs);
IMU.setAccelCalY(ayb,ays);
IMU.setAccelCalZ(azb,azs);
IMU.setMagCalX(hxb,hxs);
IMU.setMagCalY(hyb,hys);
IMU.setMagCalZ(hzb,hzs);
}
Thanks to everybody for taking your time and giving out valuable advices.
Boris