JA7MLC 電子工作ブログ: ドローン３号機 ESP32 9軸センサBMX055 フライト

ESP32によるドローン３号機を製作。ESP32から9軸センサBMX055を読込んで、Madgwickフィルタ・PID制御し、RemoteXYアプリ（ Bluetooth）操作による浮上テストを行いました。 Wi-Fiは接続に時間がかかるがBluetooth早く使いやすい。

●ボードマネージャ　esp32

　 by Espressif Systems バージョン3.2.0

●同ブログ【ドローン ESP32 9軸センサBMX055 机上テスト】

動画

参考サイト

●ESP32マルチ書き込みボード

【https://qiita.com/nak435/items/500c877bcaa3d38b939a】

●ESP32-WROOM-32Eを最小構成で動かす

【https://tsuzureya.net/esp32-wroom-32e-minimal-setup/】

●ESP32のGPIO出力について

【https://lang-ship.com/blog/work/esp32-gpio-output/】

回路図

ESP32-WROOM-32E

●Wi-Fiモジュール ESP32-WROOM-32E 16MB

Bluetooth V4.2

【https://akizukidenshi.com/catalog/g/g115675/】

ピッチ変換基板

●ESP-WROOM-32 ピッチ変換基板コンパクト

【https://www.amazon.co.jp/dp/B06XP3TY1B?psc=1&ref=ppx_yo2ov_dt_b_product_details】

【https://www.switch-science.com/products/4116】

基板ハンダ

丸ピンヘッダ・ソケット

●丸ピンヘッダ・ソケット2.54mmピッチ

ESP32・BMX055とドライブ基板接続

接触が気になるので、丸ピンヘッダの表面にハンダを流しました。

丸ピンヘッダハンダ

SI2302A

●MOSFETドライバー

【https://www.amazon.co.jp/gp/product/B09F9TTTD1/ref=ppx_yo_dt_b_asin_title_o09_s00?ie=UTF8&psc=1】

ADP3338

●表面実装低損失レギュレーターADP3338AKCZ-3.3

ドロップアウト電圧：190mV

【https://akizukidenshi.com/catalog/g/g112635/】

ドローンフレーム・モータ

●コアレスモーター 0716【https://www.amazon.co.jp/dp/B08NGCCCQL?psc=1&ref=ppx_yo2ov_dt_b_product_details】

●フレーム　BETAFPV　65S　65mm Micro Whoop Frame for 7x16mm Motors Version 4【https://item.rakuten.co.jp/airstage/10015759/】

●BETAFPV プロペラ HQ 31mm 3-Blade Propellers (0.8mm Shaft 4PCS) 【クリアグ

レー】（BETA 65S 2022Ver.などに）

●ドローンアクセサリー 4 ペア 31mm 0.8mm 1.0mm 穴 3 ブレードプロペラ PC 小道具 CW CCWドローンアクセサリー 4 ペア【https://www.amazon.co.jp/gp/product/B0BGM633K5/ref=ox_sc_act_title_1?smid=A28LWE3HRWLAWN&th=1】

バッテリー

●BETAFPV バッテリー PH2.0 300mAh 1S 30C Battery(2PCS)

【https://item.rakuten.co.jp/airstage/16071/】

基板

●角型ランド両面スルーホールガラスコンポジット・ユニバーサル基板 Cタイプ(72×47mm)
【https://akizukidenshi.com/catalog/g/g109747/】

フレームに合わせて切断

9軸センサBMX055

●9軸(加速度3軸+ジャイロ3軸+磁気コンパス3軸)センサ BMX055にレベル変換回路

【https://akizukidenshi.com/catalog/g/g113010/】

●I2Cアドレス

　加速度0x19 ジャイロ0x69 磁気0x13　

　JP1オープン JP2オープン JP3オープン　

●プルアップ接続

　JP4ショート JP5ショート　

●電源3.3V　信号3.3V　

　JP6オープン　JP7ショート　JP8オープン

BMX055取付

ESP32基板の裏にBMX055基板を45度傾けて両面テープ固定。

基板ハンダ

LED取付

抵抗内臓LEDをIO12・ IO27穴にハンダ

プログラム制御による点灯

IO12 LED　⊕

IO27 LED　⊖

秋月電子通商

【https://akizukidenshi.com/catalog/g/g116692/】

CH9102搭載 ESP32 ダウンローダキット

CP2104/CH9102搭載 ESP32 ダウンローダキット【https://akizukidenshi.com/catalog/g/g117240/】

ドライバーインストール

●ESP32 Downloader Kitダウンロードサイト【https://docs.m5stack.com/en/accessory/esp32_downloader_kit】

●【CH9102_VCP_SER_Windows】選択してドライバーをダウンロードする

●ドライバーをインストールする

操作

プッシュスイッチ操作無しでプログラム書込

み・実行を自動で行ってくれる。

プログラム書込み

変換コネクタ製作

ピンソケットと丸ピンヘッダをはんだし、チューブを熱収縮する（ピンヘッダの表面にハンダを流す）

モータケーブルを基板にハンダ付けし、フレームにESP32コントローラを取付けで固定する。（輪ゴム→ポリウレタン Puバンドに変更

）

モータ位置

0716モーター

■CWモーター（時計回転）

赤⊕　黒⊖

M1 M3

■CCWモーター（反時計回転）

白⊕　黒⊖

M2 M4

上に移動

M１・M２の回転を早く

M３・M４の回転を遅く

左に移動

M１・M４の回転を早く

M２・M３の回転を遅く

右に回転

M2・M４の回転を早く

M1・M３の回転を遅く

RemoteXYアプリ

●同ブログ関連記事：【RemoteXY使い方】

【Properties】【Configuration】設定変更

［Connection］Bluetooth

［Board］ESP32 based board

［Module］Integrated Bluetooth

【Apply】実行

【Slider】をスマホ画面に貼付け、大きさ・色等を変更

（コントロール配置は５個以内）

【Get source code】実行

コードをArduino IDE　エディタ画面に貼付てスケッチを行う。

両手で旋回と上昇を操作する配置

ジョイスティック 1（右）　↕上昇／下降　↔左移動／右移動

ジョイスティック 2（左）　↕前進／後進　↔左旋回／右旋回

ON：モータ起動

C：キャリブレーション

プログラム Arduino IDE【ボード：ESP32 Dev Module】

//ESP32ボードマネージャ バージョン3.2.0
//////////////////////////////////////////////
//        RemoteXY include library          //
//////////////////////////////////////////////

// you can enable debug logging to Serial at 115200
//#define REMOTEXY__DEBUGLOG    

// RemoteXY select connection mode and include library 
#define REMOTEXY_MODE__ESP32CORE_BLE

#include <BLEDevice.h>

// RemoteXY connection settings 
#define REMOTEXY_BLUETOOTH_NAME "RemoteXY"


#include <RemoteXY.h>

// RemoteXY GUI configuration  
#pragma pack(push, 1)  
uint8_t RemoteXY_CONF[] =   // 65 bytes
  { 255,6,0,0,0,58,0,19,0,0,0,0,83,1,200,84,1,1,4,0,
  5,120,4,79,79,0,2,26,31,5,1,4,79,79,0,2,26,31,10,84,
  52,33,30,49,4,26,31,79,78,0,31,79,70,70,0,1,84,4,33,30,
  1,2,31,67,0 };
  
// this structure defines all the variables and events of your control interface 
struct {

    // input variables
  int8_t joystick_01_x; // from -100 to 100
  int8_t joystick_01_y; // from -100 to 100
  int8_t joystick_02_x; // from -100 to 100
  int8_t joystick_02_y; // from -100 to 100
  uint8_t pushSwitch_01; // =1 if state is ON, else =0
  uint8_t button_01; // =1 if button pressed, else =0

    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0

} RemoteXY;   
#pragma pack(pop)
 
/////////////////////////////////////////////
//           END RemoteXY include          //
/////////////////////////////////////////////



#include<Wire.h>
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

float ROLL_P, ROLL_I, ROLL_D, ROLL_M;
float PITCH_P, PITCH_I, PITCH_D, PITCH_M;
float YAW_P, YAW_I, YAW_D, YAW_M;
float R, P, Y;
float Kp, Kd, Ki;
int throttle, Jx1, Jy1, Jx2, Jy2, Auto;

#define M1 18
#define M2 19
#define M3 23
#define M4 32

// BMX055
#define Addr_Accl 0x19  // 加速度センサのI2Cアドレス(JP1,JP2,JP3 = Openの時)
#define Addr_Gyro 0x69  // ジャイロセンサのI2Cアドレス(JP1,JP2,JP3 = Openの時)
#define Addr_Mag 0x13   // 磁気センサのI2Cアドレス(JP1,JP2,JP3 = Openの時)

// センサーの値を保存するグローバル変数
float ax = 0.00;//加速度
float ay = 0.00;
float az = 0.00;
float gx = 0.00;//角速度
float gy = 0.00;
float gz = 0.00;
int mx = 0;
int my = 0;
int mz = 0;
float ROLL = 0.00;
float PITCH = 0.00;
float YAW = 0.00;
int m1, m2, m3, m4;
void BMX055_Init() //BMX055の初期化
{
  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 0x00 //100Hz 0x07
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4E);  // Select Mag register
  Wire.write(0x84);  // X, Y, Z-is enabled
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x51);  // Select Mag register
  Wire.write(0x04);  // No. of Repetitions for X-Y is = 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_All() //BMX055から全データとMadgwickフィルタの結果を取得する
{
  unsigned int data[8];

  //加速度データを取得する
  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
    // ax lsb, ax msb, ay lsb, ay msb, az lsb, az msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  ax = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (ax > 2047)  ax -= 4096;
  ay = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (ay > 2047)  ay -= 4096;
  az = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (az > 2047)  az -= 4096;
  ax = ax * 0.0098; // range = +/-2g
  ay = ay * 0.0098; // range = +/-2g
  az = az * 0.0098; // range = +/-2g
  // 上記の乗数は秋月のマニュアル通りですが、誤値とのことです。
  // 正しくは、mg換算で0.98、G換算で0.00098
  // とのことです。

  //ジャイロデータを取得する
  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
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  gx = (data[1] * 256) + data[0];
  if (gx > 32767)  gx -= 65536;
  gy = (data[3] * 256) + data[2];
  if (gy > 32767)  gy -= 65536;
  gz = (data[5] * 256) + data[4];
  if (gz > 32767)  gz -= 65536;

  gx = gx * 0.0038; //  Full scale = +/- 125 degree/s
  gy = gy * 0.0038; //  Full scale = +/- 125 degree/s
  gz = gz * 0.0038; //  Full scale = +/- 125 degree/s

  //コンパスデータを取得する
  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
    // mx lsb, mx msb, my lsb, my msb, mz lsb, mz msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  mx = ((data[1] << 5) | (data[0] >> 3));
  if (mx > 4095)  mx -= 8192;
  my = ((data[3] << 5) | (data[2] >> 3));
  if (my > 4095)  my -= 8192;
  mz = ((data[5] << 7) | (data[4] >> 1));
  if (mz > 16383)  mz -= 32768;
}


void setup()
{
  pinMode(12, OUTPUT);   //LED +
  digitalWrite(12, HIGH);
  pinMode(27, OUTPUT);  
  digitalWrite(27, LOW); //LED -
  RemoteXY_Init ();
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);
  //ledcSetup(1, 1000, 8);   // モータ 0CH 490Hz 8bit=0-255(PWM)
  //ledcSetup(2, 1000, 8);
  //ledcSetup(3, 1000, 8);
  //ledcSetup(4, 1000, 8);

  //ledcAttachPin(M1, 1); // モータ CH
  //ledcAttachPin(M2, 2);
  //ledcAttachPin(M3, 3); 
  //ledcAttachPin(M4, 4);

  //ESP32ボードマネージャ バージョン3以降
  ledcAttach(M1,1000,8);
  ledcAttach(M2,1000,8);
  ledcAttach(M3,1000,8);
  ledcAttach(M4,1000,8); 

  
  Wire.begin();
  Serial.begin(115200);
  BMX055_Init(); //BMX055の初期化
  delay(300);
  MadgwickFilter.begin(100);//フィルタのサンプリングを100Hzで
  R = 0;
  P = 0;
  Y = 0;
  Kp = 0;
  Ki = 0;
  Kd = 0;

}

void loop()
{
  RemoteXY_Handler ();
  Jx1 = RemoteXY.joystick_01_x;  //左右移動
  Jy1 = RemoteXY.joystick_01_y;  //上昇下降
  Jx2 = RemoteXY.joystick_02_x;  //左右旋回
  Jy2 = RemoteXY.joystick_02_y;  //前進後進
  Auto = RemoteXY.pushSwitch_01; //自動浮上 
    
  Jx1 = map(Jx1, -100, 100, -255, 255);
  Jy1 = map(Jy1, -100, 100, -255, 255);
  Jx2 = map(Jx2, -100, 100, -255, 255);
  Jy2 = map(Jy2, -100, 100, -255, 255); 

  BMX055_All();//BMX055から全データ
  MadgwickFilter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW  = MadgwickFilter.getYaw();
  YAW  = YAW - 90;

if (RemoteXY.button_01 == 1) { //"CALボタン"
    R = ROLL;
    P = PITCH;
    Y = YAW;
    ROLL_M = 0;
    PITCH_M = 0;
    YAW_M = 0;
  }

  ROLL  = ROLL - R; //キャリブレーション
  PITCH  = PITCH - P;
  YAW  = YAW - Y;  

// PID制御
  ROLL_P = ROLL / 90; // P成分：傾き-90～90度 → -1～1
  ROLL_I += ROLL_P; // I成分：傾きの積算。
  if (10 <  abs(ROLL_I)) ROLL_I = 0; //上限設定
  ROLL_D = gx / 125; // D成分：角速度-125～125dps → -1～1
  ROLL_M = ROLL_P * Kp +
           ROLL_I * Ki +
           ROLL_D * Kd;

  PITCH_P = PITCH / 90; // P成分：傾き-90～90度 → -1～1
  PITCH_I += PITCH_P; // I成分：傾きの積算。
  if (10 < abs(PITCH_I)) PITCH_I = 0; //上限設定
  PITCH_D = gy / 125; // D成分：角速度-125～125dps → -1～1
  PITCH_M = PITCH_P * Kp +
            PITCH_I * Ki +
            PITCH_D * Kd;

  YAW_P = YAW / 90; // P成分：傾き-90～90度 → -1～1
  YAW_I += YAW_P; // I成分：傾きの積算。
  if (10 < abs(YAW_I)) YAW_I = 0; //上限設定
  YAW_D = gz / 125; // D成分：角速度-125～125dps → -1～1
  YAW_M = YAW_P * Kp +
            YAW_I * Ki +
            YAW_D * Kd;
            
  YAW_M = YAW_M + Jx2/20;
  ROLL_M = ROLL_M + Jy2/20;
  PITCH_M = PITCH_M - Jx1/20;

 //自動浮上
 if(Auto == 1 ){ 
   //定数
  Kp = 40;
  Ki = 0.1;
  Kd = 10;
  throttle = (Jy1 + 255)/2 * 0.7;
  }else{   
  throttle = 0;
  ROLL_M = 0;
  PITCH_M = 0;
  YAW_M = 0;  
}  
   
    
    // シリアルモニタ
    //Serial.print("Kp:");Serial.print(Kp); Serial.print("｜ ");
    //Serial.print("Ki:");Serial.print(Ki); Serial.print("｜ ");
    //Serial.print("Kd:");Serial.print(Kd); Serial.print("｜ ");
    //Serial.print("gx:");Serial.print(gx); Serial.print("｜ ");
    //Serial.print("gy:");Serial.print(gy); Serial.print("｜ ");
    //Serial.print("gz:");Serial.print(gz); Serial.print("｜ ");
    //Serial.print("ROLL:");Serial.print(ROLL); Serial.print("｜ ");
    //Serial.print("PITCH:");Serial.print(PITCH); Serial.print("｜ ");
    //Serial.print("YAW:");Serial.print(YAW); Serial.print("｜ ");
    //Serial.print("ROLL_P:");Serial.print(ROLL_P); Serial.print("｜ ");
    //Serial.print("ROLL_I:");Serial.print(ROLL_I); Serial.print("｜ ");
    //Serial.print("ROLL_D:");Serial.print(ROLL_D); Serial.print("｜ ");
    //Serial.print("ROLL_M:");Serial.print(ROLL_M); Serial.print("｜ ");
    //Serial.print("PITCH_P:");Serial.print(PITCH_P); Serial.print("｜ ");
    //Serial.print("PITCH_I:");Serial.print(PITCH_I); Serial.print("｜ ");
    //Serial.print("PITCH_D:");Serial.print(PITCH_D); Serial.print("｜ ");
    //Serial.print("PITCH_M:");Serial.print(PITCH_M); Serial.print("｜ ");
    //Serial.print("YAW_P:");Serial.print(YAW_P); Serial.print("｜ ");
    //Serial.print("YAW_I:");Serial.print(YAW_I); Serial.print("｜ ");
    //Serial.print("YAW_D:");Serial.print(YAW_D); Serial.print("｜ ");
    //Serial.print("YAW_M:");Serial.print(YAW_M); Serial.print("｜ ");
    //Serial.print("\n");

    //シリアルプロッタ
    //Serial.print(gx); Serial.print(",");
    //Serial.print(gy); Serial.print(",");
    //Serial.print(gz); Serial.print(",");
    Serial.print(ROLL); Serial.print(",");
    Serial.print(ROLL_P); Serial.print(",");
    Serial.print(ROLL_I); Serial.print(",");
    Serial.print(ROLL_D); Serial.print(",");
    Serial.print(ROLL_M); Serial.print(",");
    //Serial.print(PITCH); Serial.print(",");
    //Serial.print(PITCH_P); Serial.print(",");
    //Serial.print(PITCH_I); Serial.print(",");
    //Serial.print(PITCH_D); Serial.print(",");
    //Serial.print(PITCH_M); Serial.print(",");
    //Serial.print(YAW); Serial.print(",");
    //Serial.print(YAW_P); Serial.print(",");
    //Serial.print(YAW_I); Serial.print(",");
    //Serial.print(YAW_D); Serial.print(",");
    //Serial.print(YAW_M); Serial.print(",");
    Serial.print("\n");    
    
    //ledcWrite(1,(throttle + ROLL_M + PITCH_M - YAW_M)); //M1
    //ledcWrite(2,(throttle + ROLL_M - PITCH_M + YAW_M)); //M2
    //ledcWrite(3,(throttle - ROLL_M - PITCH_M - YAW_M)); //M3
    //ledcWrite(4,(throttle - ROLL_M + PITCH_M + YAW_M)); //M4
    
    //ESP32ボードマネージャ バージョン3以降
    ledcWrite(M1,(throttle + ROLL_M + PITCH_M - YAW_M)); //M1
    ledcWrite(M2,(throttle + ROLL_M - PITCH_M + YAW_M)); //M2
    ledcWrite(M3,(throttle - ROLL_M - PITCH_M - YAW_M)); //M3
    ledcWrite(M4,(throttle - ROLL_M + PITCH_M + YAW_M)); //M4
    delay(5);
}
  

JA7MLC 電子工作ブログ

2025年12月1日月曜日

ドローン３号機 ESP32 9軸センサBMX055 フライト

0 件のコメント:

コメントを投稿

ドローン３号機 ESP32 9軸センサBMX055 フライト

不正行為を報告

ラベル