2025年12月1日月曜日

ドローン ESP32 9軸センサBMX055 机上テスト

ESP32 NodeMCU開発ボードから9軸センサBMX055 を読込みMadgwickフィルタ・PID制御し、RemoteXYアプリ（Bluetooth）操作でモータの動作確認を行いました。     　
●ボードマネージャ　esp32
　 by Espressif Systems バージョン3.2.0
●同ブログ関連記事：【ESP32使い方】







動画




参考サイト
●ESP32のGPIO出力について
【https://lang-ship.com/blog/work/esp32-gpio-output/】
●姿勢計測 + PID制御で光量調節（ドローン製作準備，esp32，Arduino IDE）



回路図





















ESP-32S 開発ボード
●ESP32 NodeMCU開発ボード2.4GHz WiFi + Bluetoothデュアルモード（技適製品）
アマゾン購入








SI2302A MOSFET
SI2302A MOSFETをヘッダーピンにハンダ付け、ブレッドボードに差込できるようにしました。
アマゾン購入








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 9軸センサ





















  


 

ドローン制御
          M4                             M1
M3                                           M2
●フレーム　プロペラ　0615 モーター　












RemoteXY操作
■ジョイスティック  
　↑　M４回転　
　↓　M２回転　　　　　　　　　　
　→　M１回転　
　←　M３回転
■スライダー
　スロット制御
　モータPID制御
















シリアルプロッタ
青-ROLL　赤-PITCH　緑-YAW











青-PITCH　赤-PITCH_P　緑-PITCH_I　橙-PITCH_D　紫-PITCH_M
















RemoteXYアプリ
●同ブログ関連記事：【RemoteXY使い方】
【Properties】【Configuration】設定変更
［Connection］Bluetooth
［Board］ESP32 based board
［Module］Integrated Bluetooth
【Apply】実行
【Slider】をスマホ画面に貼付け、大きさ・色等を変更
（コントロール配置は５個以内）
【Get source code】実行
コードをArduino IDE　エディタ画面に貼付てスケッチを行う。
スマホには名称表示しません































ジョイスティック  上：M４　下：M2　
　　　　　　　　　右：M1　左：M3
スライダー01：スロットル
スライダー02：kp
スライダー03：ki x 0.01
スライダー04：Kd



ESP32バージョン（3.2.0）PWMスケッチの変更
ボードマネージャ esp32 by Espressif Systems(3.2.0)
●ESP32（3.0以降）LEDC関数でエラー発生/対応方法

  ledcSetup(1, 1000, 8); 　チャンネル、 1000Hz、 8bit=0-255(PWM)
  ledcSetup(2, 1000, 8);
  ledcSetup(3, 1000, 8);
  ledcSetup(4, 1000, 8);

  ledcAttachPin(M1, 1); 　pin、チャンネル
  ledcAttachPin(M2, 2);
  ledcAttachPin(M3, 3); 
  ledcAttachPin(M4, 4);     
         　　  ↓ 
  ledcAttach(M1,1000,8);　pin、 1000Hz、 8bit=0-255(PWM)
  ledcAttach(M2,1000,8);
  ledcAttach(M3,1000,8);
  ledcAttach(M4,1000,8);  

 ledcWrite(1,(S1 * 0.9 + m1 * 0.05 + ROLL_M + PITCH_M - YAW_M)); //M1 　         
 ledcWrite(2,(S1 * 0.9 + m2 * 0.05 + ROLL_M - PITCH_M + YAW_M)); //M2
 ledcWrite(3,(S1 * 0.9 + m3 * 0.05 - ROLL_M - PITCH_M - YAW_M)); //M3
 ledcWrite(4,(S1 * 0.9 + m4 * 0.05 - ROLL_M + PITCH_M + YAW_M)); //M4    
                    ↓        
 ledcWrite(M1,(S1 * 0.9 + m1 * 0.05 + ROLL_M + PITCH_M - YAW_M)); //M1
 ledcWrite(M2,(S1 * 0.9 + m2 * 0.05 + ROLL_M - PITCH_M + YAW_M)); //M2
 ledcWrite(M3,(S1 * 0.9 + m3 * 0.05 - ROLL_M - PITCH_M - YAW_M)); //M3
 ledcWrite(M4,(S1 * 0.9 + m4 * 0.05 - ROLL_M + PITCH_M + YAW_M)); //M4


プログラム　Arduino IDE【ボード：NodeMCU-32s】
//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[] =   // 60 bytes
  { 255,6,0,0,0,53,0,17,0,0,0,81,1,106,200,1,1,5,0,4,
  8,114,94,16,128,162,26,4,8,135,94,16,128,190,26,4,8,155,94,16,
  128,190,26,5,1,4,104,104,0,2,26,31,4,8,175,94,16,128,190,26 };
 
// this structure defines all the variables and events of your control interface
struct {

    // input variables
  int8_t slider_01; // from 0 to 100
  int8_t slider_02; // from 0 to 100
  int8_t slider_03; // from 0 to 100
  int8_t joystick_01_x; // from -100 to 100
  int8_t joystick_01_y; // from -100 to 100
  int8_t slider_04; // from 0 to 100

    // 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;

#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で


}

void loop()
{
  RemoteXY_Handler ();
  int X = RemoteXY.joystick_01_x;
  int Y = RemoteXY.joystick_01_y;
 
  int S1 = RemoteXY.slider_01;  //スロットル
  int S2 = RemoteXY.slider_02;  //Kp
  int S3 = RemoteXY.slider_03;  //Ki
  int S4 = RemoteXY.slider_04;  //Kd
 

  S1 = map(S1, 0, 100, 0, 255);  
  X = map(X, -100, 100, -255, 255);// + m1 - m3
  Y = map(Y, -100, 100, -255, 255);// + m4 - m2

  if( X < 0){
    m3 = abs(X);
  }else{
    m1 = X;
  }

   if( Y < 0){
    m2 = abs(Y);
  }else{
    m4 = Y;
  }

  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;

  //定数
  Kp = S2 * 1;   //1-100
  Ki = S3 * 0.01; //0.01-1
  Kd = S4 * 1;  //1-100  

    // 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;

    // シリアルモニタ
    //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,(S1 * 0.9 + m1 * 0.05 + ROLL_M + PITCH_M - YAW_M)); //M1
    //ledcWrite(2,(S1 * 0.9 + m2 * 0.05 + ROLL_M - PITCH_M + YAW_M)); //M2
    //ledcWrite(3,(S1 * 0.9 + m3 * 0.05 - ROLL_M - PITCH_M - YAW_M)); //M3
    //ledcWrite(4,(S1 * 0.9 + m4 * 0.05 - ROLL_M + PITCH_M + YAW_M)); //M4
   
    //ESP32バージョン3以降
    ledcWrite(M1,(S1 * 0.9 + m1 * 0.05 + ROLL_M + PITCH_M - YAW_M)); //M1
    ledcWrite(M2,(S1 * 0.9 + m2 * 0.05 + ROLL_M - PITCH_M + YAW_M)); //M2
    ledcWrite(M3,(S1 * 0.9 + m3 * 0.05 - ROLL_M - PITCH_M - YAW_M)); //M3
    ledcWrite(M4,(S1 * 0.9 + m4 * 0.05 - ROLL_M + PITCH_M + YAW_M)); //M4
}
時刻:
ラベル: , , ,

0 件のコメント:

コメントを投稿

登録: コメントの投稿 (Atom)

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

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

  • Pi Pico Rx - SDR試作 7MHz SSB CW受信
    Pi Pico Rx(0-30MHz,CW/SSB/AM/FM)シンプルSDRを試作し、注目したPico PIOアッセンブルによるIQ局発と直交ミキサの構成で7MHz SSB CWの受信を行いました（ FMは29MHzFMモードで受信確認） 同ブログ関連記事 【 Pi Pico ...
  • Pi Pico Rx - SDR試作 AM放送受信
    Pi Pico Rx(0-30MHz,CW/SSB/AM/FM)シンプルSDRを試作し、注目したPico PIOアッセンブルによるIQ局発と直交ミキサの構成でAM放送の受信を行いました。  同ブログ関連記事 【 Pi Pico Rx - SDR試作 14MHz FT8受信 】 【...
  • uSDXトランシーバー製作（受信）
    Arduinoを使ってuSDXトランシーバー製作、こちらのブログを参考にしました。【 http://ja2gqp.blogspot.com/2021/ 】 【 https://hf5l.pl/en/transceiver-usdx-with-arduino-nano/ 】 JA2...