DIY Drone - ESP32 [Version 2] #4 (How to detect Drone angle) Source code

#include "BluetoothSerial.h"
#include<math.h>
#define Deg2rad 3.1415 /180
#include <MadgwickAHRS.h>

//MPU9250関連
#include <Wire.h>
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ;

//取得データの実単位化スケールファクター
float SF_Acc = 16384; //加速度のスケールファクタ   (digit/g) 
float SF_Gy  = 131; //#ジャイロのスケールファクタ  (digit/dps)
float SF_Mg  = 500; //磁気のスケールファクタ(500☛0.6かも。。)
float SF_Tmp = 333.87; //温度のスケールファクタ
float g2mpss = 9.80665; //Gをm/s2に変換
float deg2rad = 3.14159265/180; //ディグリーをラジアンに

int i;

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

BluetoothSerial SerialBT;
Madgwick MadgwickFilter;

///タイマー割込み 参考https://55life555.blog.fc2.com/blog-entry-3194.html
volatile int timeCounter1;
hw_timer_t *timer1 = NULL; 
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
void IRAM_ATTR onTimer1(){

  portENTER_CRITICAL_ISR(&timerMux);
  timeCounter1++;
  portEXIT_CRITICAL_ISR(&timerMux);
}

void setup() {
  //Bluetooth
  Serial.begin(115200);
  SerialBT.begin("ESP32test"); //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");

  //Brushlessmotor
//  pinMode(Brushless_Motor_PIN, OUTPUT);//Brushless_Motor_PIN as OUTPUT
//  ledcSetup(0, 1000, 8);//PWM channel is 0, Frequency 1000Hz, 8 bit(Duty with 256 levels of resolution, the value is 0-255)
//  ledcAttachPin(Brushless_Motor_PIN,0);//Assign channel 0 to Brushless_Motor_PIN

//MPU9250 set up
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  setupMPU9255();

  //MadgwickFilter
  MadgwickFilter.begin(10);//filtering frequency 100Hz

//タイマー割込み
  timer1 = timerBegin(0, 80, true);
  timerAttachInterrupt(timer1, &onTimer1, true);
  timerAlarmWrite(timer1, 100000, true);
  timerAlarmEnable(timer1);
}

///通信データの正しさの確認
uint8_t calculate_checksum(uint8_t *data) {
  uint8_t checksum = 0;
  checksum |= 0b11000000 & data[1];
  checksum |= 0b00110000 & data[2];
  checksum |= 0b00001100 & data[3];
  checksum |= 0b00000011 & data[4];
  return checksum;
}

//センサー値スケール化後数値
float AcX_SF;
float AcY_SF;
float AcZ_SF;
float GyX_SF;
float GyY_SF;
float GyZ_SF;

//変数
struct strc{
  float AXIS_X;
  float AXIS_Y;
  float AXIS_Z;
};

float ROLL, PITCH, YAW;

void setupMPU9255() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}

//MPU9250セットアップ関連
void readCompass() {
  Wire.beginTransmission(0x0C);
  Wire.write(0x02);
  Wire.endTransmission();
  Wire.requestFrom(0x0C, 1);

  uint8_t ST1 = Wire.read();
  if (ST1 & 0x01) {
    Wire.beginTransmission(0x0C);
    Wire.write(0x03);
    Wire.endTransmission();
    Wire.requestFrom(0x0C, 7);
    uint8_t i = 0;
    uint8_t buf[7];
    while (Wire.available()) {
      buf[i++] = Wire.read();
    }
    if (!(buf[6] & 0x08)) {
      MgX = ((int16_t)buf[1] << 8) | buf[0];
      MgY = ((int16_t)buf[3] << 8) | buf[2];
      MgZ = ((int16_t)buf[5] << 8) | buf[4];
    }
  }
}

void loop() {
unsigned long time;//「time」をunsigned longで変数宣言, declared "time"as variable
time = millis();//プログラム実行からの経過時間(ms)をtimeに返す,Returns from program execution lapsed time (ms) to "time" 

if (timeCounter1 > 0) {
  portENTER_CRITICAL(&timerMux);
  timeCounter1--;
  portEXIT_CRITICAL(&timerMux);

  ////MPU9250関連////
  //読み出し, read
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,16,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  readCompass();

  //実単位化(Acc/g->m/s^2,Gy/dps->rad/s)
  MadgwickFilter.update(GyX/SF_Gy, GyY/SF_Gy, GyZ/SF_Gy, AcX/SF_Acc, AcY/SF_Acc, AcZ/SF_Acc, MgX/SF_Mg, MgY/SF_Mg, MgZ/SF_Mg);
  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW  = MadgwickFilter.getYaw();

  //書き出し
  Serial.print(ROLL); Serial.print(",");
  Serial.print(PITCH); Serial.print(",");
  Serial.print(YAW);
  Serial.print("\n");
  //Serial.print(AcZ/SF_Acc*g2mpss);
  delay(10);
  
///////////////////////////////////////////  
  uint8_t recv_data[6];
  if (SerialBT.available()) {
    SerialBT.readBytes(recv_data, 6);
    
    if (recv_data[0] != 'T') {
      Serial.print("Receive error!");
      return;
    }

    if (recv_data[5] != calculate_checksum(recv_data)) {
      Serial.print("Decode error!");
      return;
    }
    Serial.printf("left_x: %d, left_y: %d, right_x: %d, right_y: %d\n", recv_data[1], recv_data[2], recv_data[3], recv_data[4]);
    if((recv_data[2] < 50)&&(i < 200)){//Duty MAX About 0.5(≒124/255)
      ledcWrite(0, i++);
    }
    if((recv_data[2] > 200)&&(i >= 1)){
      ledcWrite(0, i--);
    }
    Serial.printf("i:%d\n", i);
  }
  delay(20);
}
}