ESP32[15] BMX055 9軸センサーのデータをBLEで送信する。
mack0113BMX055 9軸センサーをRaspberry Piで読んで姿勢推定に使おうと思っていましたが、９軸センサーから姿勢を出すことが難しいことがわかりました。しかし、ArduinoだとMadgwickライブラリーが提供されていて簡単に姿勢推定ができることもわかり、急遽、ESP32で姿勢推定をするように方針変換しました。最終的には、SIM⇒Softether⇒自席でデータを見ることが目的なので、このESP32で計算した姿勢推定値をラズパイに渡す必要があります。今まではBluetooth SerialでBluetoothを使ったことはありましたが、やはりここはBLEでデータを送るのが良いだろうなということで初めてBLEサーバーを使ってみました。スケッチ例にESP32のBLE_Server.ino があるのでこれを改造してます。BMX055とMadgwickの使い方は下記のページのほぼコピーです。
//================================================================//
// AE-BMX055 ESP32 //
// VCC +5V //
// GND GND //
// SDA DIO21 (I2C SDA) //
// SCL DIO22 (I2C SCL) //
// //
// (JP4,JP5,JP6はショートした状態) //
// http://akizukidenshi.com/catalog/g/gK-13010/ //
//================================================================//
/*
Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleServer.cpp
Ported to Arduino ESP32 by Evandro Copercini
updates by chegewara
*/
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include<Wire.h>
#include <MadgwickAHRS.h>
#include <math.h>
Madgwick MadgwickFilter;
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#define SERVICE_UUID "D5875408-FA51-4763-A75D-7D33CECEBC31"
#define CHARACTERISTIC_UUID "A4F01D8C-A037-43B6-9050-1876A8C23584"
#define Addr_Accl 0x19 // (JP1,JP2,JP3 = Open)
#define Addr_Gyro 0x69 // (JP1,JP2,JP3 = Open)
#define Addr_Mag 0x13 // (JP1,JP2,JP3 = Open)
#define GRYO 1
#define ACCL 2
#define MAG 3
#define XYZ_ROTATION 4
float xAccl = 0.00;
float yAccl = 0.00;
float zAccl = 0.00;
float xGyro = 0.00;
float yGyro = 0.00;
float zGyro = 0.00;
float xMag = 0;
float yMag = 0;
float zMag = 0;
float roll = 0 ;
float pitch = 0 ;
float yaw = 0 ;
//=====================================================================================//
void BMX055_Init()
{
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Accl);
Wire.write(0x0F); // Select PMU_Range register
Wire.write(0x03); // Range = +/- 2g
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Accl);
Wire.write(0x10); // Select PMU_BW register
Wire.write(0x08); // Bandwidth = 7.81 Hz
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Accl);
Wire.write(0x11); // Select PMU_LPW register
Wire.write(0x00); // Normal mode, Sleep duration = 0.5ms
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Gyro);
Wire.write(0x0F); // Select Range register
Wire.write(0x04); // Full scale = +/- 125 degree/s
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Gyro);
Wire.write(0x10); // Select Bandwidth register
Wire.write(0x07); // ODR = 100 Hz
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Gyro);
Wire.write(0x11); // Select LPM1 register
Wire.write(0x00); // Normal mode, Sleep duration = 2ms
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Mag);
Wire.write(0x4B); // Select Mag register
Wire.write(0x83); // Soft reset
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Mag);
Wire.write(0x4B); // Select Mag register
Wire.write(0x01); // Soft reset
Wire.endTransmission();
delay(100);
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Mag);
Wire.write(0x4C); // Select Mag register
Wire.write(0x00); // Normal Mode, ODR = 10 Hz
Wire.endTransmission();
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Mag);
Wire.write(0x4E); // Select Mag register
Wire.write(0x84); // X, Y, Z-Axis enabled
Wire.endTransmission();
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Mag);
Wire.write(0x51); // Select Mag register
Wire.write(0x04); // No. of Repetitions for X-Y Axis = 9
Wire.endTransmission();
//------------------------------------------------------------//
Wire.beginTransmission(Addr_Mag);
Wire.write(0x52); // Select Mag register
Wire.write(0x16); // No. of Repetitions for Z-Axis = 15
Wire.endTransmission();
}
//=====================================================================================//
void BMX055_Accl()
{
int data[6];
for (int i = 0; i < 6; i++)
{
Wire.beginTransmission(Addr_Accl);
Wire.write((2 + i));// Select data register
Wire.endTransmission();
Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
// Read 6 bytes of data
// xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
if (Wire.available() == 1)
data[i] = Wire.read();
}
// Convert the data to 12-bits
xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
if (xAccl > 2047) xAccl -= 4096;
yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
if (yAccl > 2047) yAccl -= 4096;
zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
if (zAccl > 2047) zAccl -= 4096;
xAccl = xAccl * 0.0098; // renge +-2g
yAccl = yAccl * 0.0098; // renge +-2g
zAccl = zAccl * 0.0098; // renge +-2g
}
//=====================================================================================//
void BMX055_Gyro()
{
int data[6];
for (int i = 0; i < 6; i++)
{
Wire.beginTransmission(Addr_Gyro);
Wire.write((2 + i)); // Select data register
Wire.endTransmission();
Wire.requestFrom(Addr_Gyro, 1); // Request 1 byte of data
// Read 6 bytes of data
// xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
if (Wire.available() == 1)
data[i] = Wire.read();
}
// Convert the data
xGyro = (data[1] * 256) + data[0];
if (xGyro > 32767) xGyro -= 65536;
yGyro = (data[3] * 256) + data[2];
if (yGyro > 32767) yGyro -= 65536;
zGyro = (data[5] * 256) + data[4];
if (zGyro > 32767) zGyro -= 65536;
xGyro = xGyro * 0.0038; // Full scale = +/- 125 degree/s
yGyro = yGyro * 0.0038; // Full scale = +/- 125 degree/s
zGyro = zGyro * 0.0038; // Full scale = +/- 125 degree/s
}
//=====================================================================================//
void BMX055_Mag()
{
int data[8];
for (int i = 0; i < 8; i++)
{
Wire.beginTransmission(Addr_Mag);
Wire.write((0x42 + i)); // Select data register
Wire.endTransmission();
Wire.requestFrom(Addr_Mag, 1); // Request 1 byte of data
// Read 6 bytes of data
// xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
if (Wire.available() == 1)
data[i] = Wire.read();
}
// Convert the data
xMag = ((data[1] << 8) | (data[0] >> 3));
if (xMag > 4095) xMag -= 8192;
yMag = ((data[3] << 8) | (data[2] >> 3));
if (yMag > 4095) yMag -= 8192;
zMag = ((data[5] << 8) | (data[4] >> 3));
if (zMag > 16383) zMag -= 32768;
}
BLEServer *pServer;
BLEService *pService;
BLECharacteristic *pCharacteristic;
void bele_server_setup() {
BLEDevice::init("BLE-YTL");
pServer = BLEDevice::createServer();
pService = pServer->createService(SERVICE_UUID);
pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE
);
pService->start();
// BLEAdvertising *pAdvertising = pServer->getAdvertising(); // this still is working for backward compatibility
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(true);
pAdvertising->setMinPreferred(0x06); // functions that help with iPhone connections issue
pAdvertising->setMinPreferred(0x12);
BLEDevice::startAdvertising();
Serial.println("This server(Peripheral). Characteristic defined! Now you can read it in your phone!");
}
void setup()
{
// Wire(Arduino-I2C)
Wire.begin();
// 115200bps
Serial.begin(115200);
//BMX055
BMX055_Init();
delay(300);
MadgwickFilter.begin(10);
// ble_server set up
bele_server_setup();
}
void loop()
{
char ble_buf[1256];
BMX055_Gyro();
Serial.print("GYO:");//gryo==1
Serial.print(" x>");
Serial.print(xGyro);
Serial.print(" y>");
Serial.print(yGyro);
Serial.print(" z>");
Serial.println(zGyro);
BMX055_Accl();
Serial.print("ACL:");//accl ==2
Serial.print(" x>");
Serial.print(xAccl);
Serial.print(" y>");
Serial.print(yAccl);
Serial.print(" z>");
Serial.println(zAccl);
BMX055_Mag();
Serial.print("MAG:");//mag==3
Serial.print(" x>");
Serial.print(xMag);
Serial.print(" y>");
Serial.print(yMag);
Serial.print(" z>");
Serial.println(zMag);
MadgwickFilter.update(xGyro, yGyro, zGyro, xAccl, yAccl, zAccl, xMag, yMag, zMag);
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw();
Serial.print("ROT:");//XYZ_ROTATION=4
Serial.print(" r>");
Serial.print(roll);
Serial.print(" p>");
Serial.print(pitch);
Serial.print(" y>");
Serial.println(yaw);
Serial.print("\n");
//Set sensor data to ble_buf
// sprintf(ble_buf, "GYO_x:%f", xGyro);
sprintf(ble_buf, "GYO_x:%f GYO_y:%f GYO_z:%f ACL_x:%f ACL_y:%f ACL_z:%f MAG_x:%f MAG_y:%f MAG_z:%f ROLL:%f PICH %f YAW:%f\n",
xGyro, yGyro, zGyro, xAccl, yAccl, zAccl, roll, pitch, yaw );
Serial.printf("%s\n", ble_buf);
pCharacteristic->setValue(ble_buf);
delay(2000);
}}
