ESP32 Forum

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);
 }