|
@@ -1,121 +1,116 @@
|
|
|
#include <BLEDevice.h>
|
|
#include <BLEDevice.h>
|
|
|
#include <BLEUtils.h>
|
|
#include <BLEUtils.h>
|
|
|
#include <BLEServer.h>
|
|
#include <BLEServer.h>
|
|
|
-
|
|
|
|
|
-// See the following for generating UUIDs:
|
|
|
|
|
-// https://www.uuidgenerator.net/
|
|
|
|
|
|
|
+#include <Wire.h>
|
|
|
|
|
+#include <math.h>
|
|
|
|
|
|
|
|
#define SERVICE_UUID "d86aecf2-d87d-489f-b664-b02de82b2fc0"
|
|
#define SERVICE_UUID "d86aecf2-d87d-489f-b664-b02de82b2fc0"
|
|
|
#define CHARACTERISTIC_UUID "d86aecf2-d87d-489f-b664-b02de82b2fc0"
|
|
#define CHARACTERISTIC_UUID "d86aecf2-d87d-489f-b664-b02de82b2fc0"
|
|
|
|
|
|
|
|
-#include <Wire.h>
|
|
|
|
|
-
|
|
|
|
|
long accelX, accelY, accelZ;
|
|
long accelX, accelY, accelZ;
|
|
|
float gForceX, gForceY, gForceZ;
|
|
float gForceX, gForceY, gForceZ;
|
|
|
|
|
+float accelMag;
|
|
|
|
|
|
|
|
long gyroX, gyroY, gyroZ;
|
|
long gyroX, gyroY, gyroZ;
|
|
|
float rotX, rotY, rotZ;
|
|
float rotX, rotY, rotZ;
|
|
|
|
|
+float gyroMag;
|
|
|
|
|
+
|
|
|
|
|
+int16_t tempRaw;
|
|
|
|
|
+float temperature;
|
|
|
|
|
+
|
|
|
|
|
+float pitch, roll;
|
|
|
|
|
|
|
|
BLECharacteristic *pCharacteristic = NULL;
|
|
BLECharacteristic *pCharacteristic = NULL;
|
|
|
|
|
|
|
|
void setup() {
|
|
void setup() {
|
|
|
Serial.begin(115200);
|
|
Serial.begin(115200);
|
|
|
- BLEDevice::init("Long name works now");
|
|
|
|
|
|
|
+
|
|
|
|
|
+ // BLE setup
|
|
|
|
|
+ BLEDevice::init("MPU6050_BLE");
|
|
|
BLEServer *pServer = BLEDevice::createServer();
|
|
BLEServer *pServer = BLEDevice::createServer();
|
|
|
BLEService *pService = pServer->createService(SERVICE_UUID);
|
|
BLEService *pService = pServer->createService(SERVICE_UUID);
|
|
|
- pCharacteristic =
|
|
|
|
|
- pService->createCharacteristic(CHARACTERISTIC_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
|
|
|
|
|
|
|
|
|
|
- pCharacteristic->setValue("Hello World says Neil");
|
|
|
|
|
|
|
+ pCharacteristic = pService->createCharacteristic(
|
|
|
|
|
+ CHARACTERISTIC_UUID,
|
|
|
|
|
+ BLECharacteristic::PROPERTY_READ |
|
|
|
|
|
+ BLECharacteristic::PROPERTY_WRITE |
|
|
|
|
|
+ BLECharacteristic::PROPERTY_NOTIFY
|
|
|
|
|
+ );
|
|
|
|
|
+
|
|
|
pService->start();
|
|
pService->start();
|
|
|
- // BLEAdvertising *pAdvertising = pServer->getAdvertising(); // this still is working for backward compatibility
|
|
|
|
|
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
|
|
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
|
|
|
pAdvertising->addServiceUUID(SERVICE_UUID);
|
|
pAdvertising->addServiceUUID(SERVICE_UUID);
|
|
|
pAdvertising->setScanResponse(true);
|
|
pAdvertising->setScanResponse(true);
|
|
|
- pAdvertising->setMinPreferred(0x06); // functions that help with iPhone connections issue
|
|
|
|
|
|
|
+ pAdvertising->setMinPreferred(0x06);
|
|
|
pAdvertising->setMinPreferred(0x12);
|
|
pAdvertising->setMinPreferred(0x12);
|
|
|
BLEDevice::startAdvertising();
|
|
BLEDevice::startAdvertising();
|
|
|
- Serial.println("Characteristic defined! Now you can read it in your phone!");
|
|
|
|
|
|
|
+
|
|
|
|
|
+ Serial.println("BLE advertising started");
|
|
|
|
|
|
|
|
Wire.begin();
|
|
Wire.begin();
|
|
|
setupMPU();
|
|
setupMPU();
|
|
|
- Serial.println("HELLO WORLD");
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-void loop(){
|
|
|
|
|
|
|
+void loop() {
|
|
|
recordAccelRegisters();
|
|
recordAccelRegisters();
|
|
|
recordGyroRegisters();
|
|
recordGyroRegisters();
|
|
|
|
|
+ recordTemp();
|
|
|
|
|
+ computeOrientation();
|
|
|
|
|
+ computeMagnitudes();
|
|
|
|
|
|
|
|
- String data = printData();
|
|
|
|
|
|
|
+ String data = formatData();
|
|
|
Serial.println(data);
|
|
Serial.println(data);
|
|
|
- pCharacteristic->setValue(data);
|
|
|
|
|
-
|
|
|
|
|
- /*
|
|
|
|
|
-
|
|
|
|
|
|
|
|
|
|
|
|
+ pCharacteristic->setValue(data.c_str());
|
|
|
|
|
+ pCharacteristic->notify();
|
|
|
|
|
|
|
|
- if(client.available()){
|
|
|
|
|
- char text = client.read();
|
|
|
|
|
- request += text;
|
|
|
|
|
-
|
|
|
|
|
- if (text == '\n'){
|
|
|
|
|
- if (request.indexOf("GET /data") != -1){
|
|
|
|
|
-… client.print("<html><body><p>");
|
|
|
|
|
- client.print(dataArray);
|
|
|
|
|
- client.println("</p></body></html>");
|
|
|
|
|
- client.println("Connection: close");
|
|
|
|
|
- client.println(dataArray);
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- }
|
|
|
|
|
- }
|
|
|
|
|
- //printData();
|
|
|
|
|
- }
|
|
|
|
|
- */
|
|
|
|
|
|
|
+ delay(200); // Update rate
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-void setupMPU(){
|
|
|
|
|
- Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
|
|
|
|
|
- Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
|
|
|
|
|
- Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
|
|
|
|
|
|
|
+void setupMPU() {
|
|
|
|
|
+ Wire.beginTransmission(0x68);
|
|
|
|
|
+ Wire.write(0x6B); // Power management
|
|
|
|
|
+ Wire.write(0);
|
|
|
Wire.endTransmission();
|
|
Wire.endTransmission();
|
|
|
- Wire.beginTransmission(0b1101000); //I2C address of the MPU
|
|
|
|
|
- Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
|
|
|
|
|
- Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
|
|
|
|
|
|
|
+
|
|
|
|
|
+ Wire.beginTransmission(0x68);
|
|
|
|
|
+ Wire.write(0x1B); // Gyro config
|
|
|
|
|
+ Wire.write(0);
|
|
|
Wire.endTransmission();
|
|
Wire.endTransmission();
|
|
|
- Wire.beginTransmission(0b1101000); //I2C address of the MPU
|
|
|
|
|
- Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
|
|
|
|
|
- Wire.write(0b00000000); //Setting the accel to +/- 2g
|
|
|
|
|
|
|
+
|
|
|
|
|
+ Wire.beginTransmission(0x68);
|
|
|
|
|
+ Wire.write(0x1C); // Accel config
|
|
|
|
|
+ Wire.write(0);
|
|
|
Wire.endTransmission();
|
|
Wire.endTransmission();
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void recordAccelRegisters() {
|
|
void recordAccelRegisters() {
|
|
|
- Wire.beginTransmission(0b1101000); //I2C address of the MPU
|
|
|
|
|
- Wire.write(0x3B); //Starting register for Accel Readings
|
|
|
|
|
|
|
+ Wire.beginTransmission(0x68);
|
|
|
|
|
+ Wire.write(0x3B);
|
|
|
Wire.endTransmission();
|
|
Wire.endTransmission();
|
|
|
- Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
|
|
|
|
|
- while(Wire.available() < 6);
|
|
|
|
|
- accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
|
|
|
|
|
- accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
|
|
|
|
|
- accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
|
|
|
|
|
|
|
+ Wire.requestFrom(0x68, 6);
|
|
|
|
|
+ while (Wire.available() < 6);
|
|
|
|
|
+ accelX = Wire.read() << 8 | Wire.read();
|
|
|
|
|
+ accelY = Wire.read() << 8 | Wire.read();
|
|
|
|
|
+ accelZ = Wire.read() << 8 | Wire.read();
|
|
|
processAccelData();
|
|
processAccelData();
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-void processAccelData(){
|
|
|
|
|
|
|
+void processAccelData() {
|
|
|
gForceX = accelX / 16384.0;
|
|
gForceX = accelX / 16384.0;
|
|
|
gForceY = accelY / 16384.0;
|
|
gForceY = accelY / 16384.0;
|
|
|
gForceZ = accelZ / 16384.0;
|
|
gForceZ = accelZ / 16384.0;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void recordGyroRegisters() {
|
|
void recordGyroRegisters() {
|
|
|
- Wire.beginTransmission(0b1101000); //I2C address of the MPU
|
|
|
|
|
- Wire.write(0x43); //Starting register for Gyro Readings
|
|
|
|
|
|
|
+ Wire.beginTransmission(0x68);
|
|
|
|
|
+ Wire.write(0x43);
|
|
|
Wire.endTransmission();
|
|
Wire.endTransmission();
|
|
|
- Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
|
|
|
|
|
- while(Wire.available() < 6);
|
|
|
|
|
- gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
|
|
|
|
|
- gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
|
|
|
|
|
- gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
|
|
|
|
|
|
|
+ Wire.requestFrom(0x68, 6);
|
|
|
|
|
+ while (Wire.available() < 6);
|
|
|
|
|
+ gyroX = Wire.read() << 8 | Wire.read();
|
|
|
|
|
+ gyroY = Wire.read() << 8 | Wire.read();
|
|
|
|
|
+ gyroZ = Wire.read() << 8 | Wire.read();
|
|
|
processGyroData();
|
|
processGyroData();
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -125,20 +120,30 @@ void processGyroData() {
|
|
|
rotZ = gyroZ / 131.0;
|
|
rotZ = gyroZ / 131.0;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-String printData() {
|
|
|
|
|
- Serial.print("Gyro (deg)");
|
|
|
|
|
- Serial.print(" X=");
|
|
|
|
|
- Serial.print(rotX);
|
|
|
|
|
- Serial.print(" Y=");
|
|
|
|
|
- Serial.print(rotY);
|
|
|
|
|
- Serial.print(" Z=");
|
|
|
|
|
- Serial.print(rotZ);
|
|
|
|
|
- Serial.print(" Accel (g)");
|
|
|
|
|
- Serial.print(" X=");
|
|
|
|
|
- Serial.print(gForceX);
|
|
|
|
|
- Serial.print(" Y=");
|
|
|
|
|
- Serial.print(gForceY);
|
|
|
|
|
- Serial.print(" Z=");
|
|
|
|
|
- Serial.println(gForceZ);
|
|
|
|
|
- return String(rotX) + "," + String(rotY) + "," + String(rotZ) + "," + String(gForceX) + "," + String(gForceY) + "," + String(gForceZ) + ",";
|
|
|
|
|
|
|
+void recordTemp() {
|
|
|
|
|
+ Wire.beginTransmission(0x68);
|
|
|
|
|
+ Wire.write(0x41);
|
|
|
|
|
+ Wire.endTransmission();
|
|
|
|
|
+ Wire.requestFrom(0x68, 2);
|
|
|
|
|
+ while (Wire.available() < 2);
|
|
|
|
|
+ tempRaw = Wire.read() << 8 | Wire.read();
|
|
|
|
|
+ temperature = (tempRaw / 340.0) + 36.53;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void computeOrientation() {
|
|
|
|
|
+ pitch = atan2(gForceY, gForceZ) * 180 / PI;
|
|
|
|
|
+ roll = atan2(-gForceX, sqrt(gForceY * gForceY + gForceZ * gForceZ)) * 180 / PI;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void computeMagnitudes() {
|
|
|
|
|
+ accelMag = sqrt(gForceX * gForceX + gForceY * gForceY + gForceZ * gForceZ);
|
|
|
|
|
+ gyroMag = sqrt(rotX * rotX + rotY * rotY + rotZ * rotZ);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+String formatData() {
|
|
|
|
|
+ return String(rotX) + "," + String(rotY) + "," + String(rotZ) + "," +
|
|
|
|
|
+ String(gForceX) + "," + String(gForceY) + "," + String(gForceZ) + "," +
|
|
|
|
|
+ String(gyroMag) + "," + String(accelMag) + "," +
|
|
|
|
|
+ String(pitch) + "," + String(roll) + "," +
|
|
|
|
|
+ String(temperature);
|
|
|
}
|
|
}
|
Lucas