Hi,
I'd like to send IMU sensor data which is gotten by one M5Stack to the other. The other M5Stack is in a few meters from one.
To achieve above, now I think use of BLE(Bluetooth Low Energy)(*1).
I'll connect two M5Stacks and send the data as data packet.
I know ESP32 which is in M5Stack enables use of Bluetooth 4.2(*2) and we can send 251 bytes(actually 246bytes because of consumption by some protocols) per time by Bluetooth 4.2(*3).
I tried, but I can't as expected. I don't know why. The problems are below.
・In the case of send and receive over 20 bytes, M5Stack receive data which is over 20bytes as zero.
・I defined data type as uint8, so I expected 1byte per one param, but actually 4bytes per one param. Did I do something wrong?
Environment
Device: M5Stack GRAY
IDE: Arduino IDE
*1 I know also ESP-NOW, but if I can use BLE, I'd like to use BLE considering security.
*2 https://www.espressif.com/sites/default/files/documentation/esp32_datasheet_en.pdf
*3 https://www.bluetooth.com/
Result
Coder for Server
#define M5STACK_MPU6886
#include <M5Stack.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLE2902.h>
/* device name */
#define DEVICE_NAME "ESP32"
/* UUID difinition */
#define SERVICE_UUID "28b0883b-7ec3-4b46-8f64-8559ae036e4e" // service UUID
#define CHARACTERISTIC_UUID_TX "2049779d-88a9-403a-9c59-c7df79e1dd7c" // UUID for send
/* for control connection */
BLECharacteristic *pCharacteristicTX; // characteristic for send
bool deviceConnected = false; // device connection status
bool bAbnormal = false; // to call abnormal
/* connection data */
struct tmpData {
int time_get;
int accX_send;
int accY_send;
int accZ_send;
int gyroX_send;
int gyroY_send;
int gyroZ_send;
int pitch_send;
int roll_send;
int yaw_send;
};
struct tmpData data;
/* IMU 9 params definition */
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;
float pitch = 0.0F;
float roll = 0.0F;
float yaw = 0.0F;
/* get time */
int time_get;
/* Callback when conncet and break */
class funcServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
}
void onDisconnect(BLEServer* pServer) {
deviceConnected = false;
}
};
/* make characteristic for Notify */
void doPrepare(BLEService * pService) {
pCharacteristicTX = pService->createCharacteristic(
CHARACTERISTIC_UUID_TX,
BLECharacteristic::PROPERTY_NOTIFY
);
pCharacteristicTX->addDescriptor(new BLE2902());
}
// Function for send data
void sendData() {
if (isnan(accX) || isnan(accY) || isnan(accZ)) {
Serial.println("Failed to read sensor!");
return;
}
// set value to struct and send
data.time_get = time_get;
data.accX_send = (int)(accX * 100);
data.accY_send = (int)(accY * 100);
data.accZ_send = (int)(accZ * 100);
data.gyroX_send = (int)(gyroX * 100);
data.gyroY_send = (int)(gyroY * 100);
data.gyroZ_send = (int)(gyroZ * 100);
data.pitch_send = (int)(pitch * 100);
data.roll_send = (int)(roll * 100);
data.yaw_send = (int)(yaw * 100);
pCharacteristicTX->setValue((uint8_t*)&data, sizeof(tmpData));
pCharacteristicTX->notify();
delay(5);
}
void setup() {
M5.begin(); //Init M5Stack.
M5.Power.begin(); //Init power.
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(3);
M5.IMU.Init();
BLEDevice::init(DEVICE_NAME);
// make service object and set Callback
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new funcServerCallbacks());
// make service object and prepare(make characteristic for Notify)
BLEService *pService = pServer->createService(SERVICE_UUID);
doPrepare(pService);
// start service and advertise
pService->start();
BLEAdvertising *pAdvertising = pServer->getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->start();
}
void loop() {
M5.IMU.getAccelData(&accX, &accY, &accZ);
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
M5.IMU.getAhrsData(&pitch, &roll, &yaw);
time_get = millis();
delay(5);
// set data and send
sendData();
delay(1000);
}
Code for Cliant
#include <M5Stack.h>
#include <BLEDevice.h>
#include <Wire.h> // For I2C interface
/* UUID difinition */
BLEUUID serviceUUID("28b0883b-7ec3-4b46-8f64-8559ae036e4e"); // service UUID
BLEUUID CHARA_UUID_RX("2049779d-88a9-403a-9c59-c7df79e1dd7c"); // UUID of RX
/* for control connection */
BLERemoteCharacteristic* pRemoteCharacteristicRX; // Characteristic for receive
BLEAdvertisedDevice* targetDevice; // target BLE device
bool doConnect = false; // connection cue
bool doScan = false; // scan cue
bool deviceConnected = false; // connection status of device
bool bInAlarm = false;
bool enableMeasurement = false;
/* connection data */
struct tmpData {
int time_get_rcv;
int accX_rcv;
int accY_rcv;
int accZ_rcv;
int gyroX_rcv;
int gyroY_rcv;
int gyroZ_rcv;
int pitch_rcv;
int roll_rcv;
int yaw_rcv;
};
struct tmpData data;
char* time_get_rcv;
char* accX_rcv;
char* accY_rcv;
char* accZ_rcv;
char* gyroX_rcv;
char* gyroY_rcv;
char* gyroZ_rcv;
char* pitch_rcv;
char* roll_rcv;
char* yaw_rcv;
/*********************< Callback classes and functions >**********************/
/* Callback when connect and break */
class funcClientCallbacks: public BLEClientCallbacks {
void onConnect(BLEClient* pClient) {
};
void onDisconnect(BLEClient* pClient) {
deviceConnected = false;
}
};
/* Callback when receive advertising */
class advertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
void onResult(BLEAdvertisedDevice advertisedDevice) {
Serial.print("Advertised Device found: ");
Serial.println(advertisedDevice.toString().c_str());
/* stop scan and prepare connect if target BLE device*/
if (advertisedDevice.haveServiceUUID()) {
BLEUUID service = advertisedDevice.getServiceUUID();
Serial.print("Have ServiceUUI: "); Serial.println(service.toString().c_str());
if (service.equals(serviceUUID)) {
BLEDevice::getScan()->stop();
targetDevice = new BLEAdvertisedDevice(advertisedDevice);
doConnect = doScan = true;
}
}
}
};
/* Callback Function of Notify */
static void notifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
uint8_t* pData, size_t length, bool isNotify) {
M5.Lcd.setCursor(0, 20);
M5.Lcd.print("Received");
memcpy(&data, pData, length);
int tm = data.time_get_rcv;
float xx = data.accX_rcv / 100.00;
float yy = data.accY_rcv / 100.00;
float zz = data.accZ_rcv / 100.00;
float gx = data.gyroX_rcv / 100.00;
float gy = data.gyroY_rcv / 100.00;
float gz = data.gyroZ_rcv / 100.00;
float pc = data.pitch_rcv / 100.00;
float rl = data.roll_rcv / 100.00;
float yw = data.yaw_rcv / 100.00;
static char time_get_rcv_char[10];
static char accX_rcv_char[10];
static char accY_rcv_char[10];
static char accZ_rcv_char[10];
static char gyroX_rcv_char[10];
static char gyroY_rcv_char[10];
static char gyroZ_rcv_char[10];
static char pitch_rcv_char[10];
static char roll_rcv_char[10];
static char yaw_rcv_char[10];
sprintf(time_get_rcv_char, "%d", tm);
sprintf(accX_rcv_char, "%5.2f", xx);
sprintf(accY_rcv_char, "%5.2f", yy);
sprintf(accZ_rcv_char, "%5.2f", zz);
sprintf(gyroX_rcv_char, "%5.2f", gx);
sprintf(gyroY_rcv_char, "%5.2f", gy);
sprintf(gyroZ_rcv_char, "%5.2f", gz);
sprintf(pitch_rcv_char, "%5.2f", pc);
sprintf(roll_rcv_char, "%5.2f", rl);
sprintf(yaw_rcv_char, "%5.2f", yw);
time_get_rcv = (char*)time_get_rcv_char;
accX_rcv = (char*)accX_rcv_char;
accY_rcv = (char*)accY_rcv_char;
accZ_rcv = (char*)accZ_rcv_char;
gyroX_rcv = (char*)gyroX_rcv_char;
gyroY_rcv = (char*)gyroY_rcv_char;
gyroZ_rcv = (char*)gyroZ_rcv_char;
pitch_rcv = (char*)pitch_rcv_char;
roll_rcv = (char*)roll_rcv_char;
yaw_rcv = (char*)yaw_rcv_char;
enableMeasurement = true;
Serial.print( millis() );
Serial.print(" ");
Serial.print("Received data: ");
Serial.print(time_get_rcv);
Serial.print(", ");
Serial.print(accX_rcv);
Serial.print(", ");
Serial.print(accY_rcv);
Serial.print(", ");
Serial.print(accZ_rcv);
Serial.print(", ");
Serial.print(gyroX_rcv);
Serial.print(", ");
Serial.print(gyroY_rcv);
Serial.print(", ");
Serial.print(gyroZ_rcv);
Serial.print(", ");
Serial.print(pitch_rcv);
Serial.print(", ");
Serial.print(roll_rcv);
Serial.print(", ");
Serial.println(yaw_rcv);
return;
}
/*****************************************************************************
Predetermined Sequence
*****************************************************************************/
void setup() {
M5.begin();
M5.Power.begin(); //Init power.
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(3);
BLEDevice::init("");
Serial.println("Client application start...");
// get Scan object and set Callback
BLEScan* pBLEScan = BLEDevice::getScan();
pBLEScan->setAdvertisedDeviceCallbacks(new advertisedDeviceCallbacks());
// scan 10 seconds
pBLEScan->setActiveScan(true);
pBLEScan->start(10);
}
void loop() {
// Connect Server once when receive Advertising
if (doConnect == true) {
if (doPrepare()) {
Serial.println("Connected to the BLE Server.");
} else {
Serial.println("Failed to connect to the BLE server.");
}
doConnect = false;
}
// If Connecting
if (deviceConnected) {
if (enableMeasurement && !bInAlarm) {
enableMeasurement = false;
}
} else if (doScan) {
BLEDevice::getScan()->start(0);
}
}
/* prepare */
bool doPrepare() {
/* make cliant object and set Callback */
BLEClient* pClient = BLEDevice::createClient();
pClient->setClientCallbacks(new funcClientCallbacks());
Serial.println(" - Created client.");
/* connect remote BLE server */
pClient->connect(targetDevice);
Serial.println(" - Connected to server.");
/* get reference to server */
BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
if (pRemoteService == nullptr) {
Serial.print("Failed to find serviceUUID: ");
Serial.println(serviceUUID.toString().c_str());
pClient->disconnect();
return false;
}
Serial.println(" - Found target service.");
/* get reference to characteristic */
pRemoteCharacteristicRX = pRemoteService->getCharacteristic(CHARA_UUID_RX);
if (pRemoteCharacteristicRX == nullptr) {
Serial.print("Failed to find characteristicUUID: ");
Serial.println(CHARA_UUID_RX.toString().c_str());
pClient->disconnect();
return false;
}
Serial.println(" - Found characteristic CHARA_UUID_RX.");
/* assign callback functon for Notify */
if (pRemoteCharacteristicRX->canNotify()) {
pRemoteCharacteristicRX->registerForNotify(notifyCallback);
Serial.println(" - Registered notify callback function.");
}
deviceConnected = true;
return true;
}