2025年11月1日土曜日

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

アプリRemoteXYとESP8266をWiFi接続しスマホからの操作と9軸センサBMX055を読込でMadgwickフィルタ・PID姿勢制御の確認を行いました。　　   






動画




参考サイト
●BMX055とESP32でMadgwickフィルタを使う


回路図


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











BMX055取付位置
M3                             M4
M2                            M1
●フレーム　プロペラ  　


















シリアルモニタ
9軸センサは6軸センサに比べてYAWのドリフトが少ない。













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















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












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


CAL：キャリブレーション（ROLL・PITCH値を０にする）
S１：Kp
S２：Ki
S３：Kd
S４：スロットル




プログラム  Arduino IDE【ボード：Generic ESP8266 Module】
//////////////////////////////////////////////
//        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__ESP8266WIFI_LIB_POINT
#include <ESP8266WiFi.h>

#include <RemoteXY.h>

// RemoteXY connection settings
#define REMOTEXY_WIFI_SSID "RemoteXY"
#define REMOTEXY_WIFI_PASSWORD "12345678"
#define REMOTEXY_SERVER_PORT 6377



// RemoteXY configurate
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =   // 63 bytes
{ 255, 5, 0, 0, 0, 56, 0, 17, 0, 0, 0, 195, 1, 126, 200, 1, 1, 5, 0, 4,
  2, 112, 122, 20, 128, 204, 26, 4, 2, 134, 122, 20, 128, 204, 26, 4, 2, 156, 122, 20,
  128, 204, 26, 4, 100, 252, 18, 114, 0, 134, 26, 1, 25, 34, 39, 38, 1, 234, 31, 67,
  65, 76, 0
};

// this structure defines all the variables and events of your control interface
struct {

  // input variables
  int8_t slider_1; // =0..100 slider position
  int8_t slider_2; // =0..100 slider position
  int8_t slider_3; // =0..100 slider position
  int8_t slider_4; // =0..100 slider position
  uint8_t button_1; // =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;

#define M1 12
#define M2 13
#define M3 14
#define M4 15

#define PWN_FREQ 1000 // PWM frequency: 1000Hz(1kHz)
#define PWN_RANGE 100 // PWN range: 100

// 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.00;
int my = 0.00;
int mz = 0.00;
float ROLL = 0.00;
float PITCH = 0.00;
float YAW = 0.00;

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()
{
  RemoteXY_Init ();
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);
  Wire.begin();
  Serial.begin(115200);
  BMX055_Init(); //BMX055の初期化
  delay(300);
  MadgwickFilter.begin(100);//フィルタのサンプリングを100Hzで
  analogWriteFreq(PWN_FREQ);
  analogWriteRange(PWN_RANGE);
  R = 0;
  P = 0;
  Y = 0;

}

void loop()
{
  RemoteXY_Handler ();
  int S1 = RemoteXY.slider_1;
  int S2 = RemoteXY.slider_2;
  int S3 = RemoteXY.slider_3;
  int S4 = RemoteXY.slider_4;

  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 = S1 * 1;   //1-100
  Ki = S2 * 0.01; //0.01-1
  Kd = S3 * 1;  //1-100

  if (RemoteXY.button_1 == 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;

  // シリアルモニタ          
   //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");
 
  analogWrite(M1, (S4 * 0.9 + ROLL_M + PITCH_M - YAW_M));
  analogWrite(M2, (S4 * 0.9 + ROLL_M - PITCH_M + YAW_M));
  analogWrite(M3, (S4 * 0.9 - ROLL_M - PITCH_M - YAW_M));
  analogWrite(M4, (S4 * 0.9 - ROLL_M + PITCH_M + YAW_M));

  delay(5);
}

時刻:
ラベル: , ,

0 件のコメント:

コメントを投稿

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

ドローン２号機 ESP8266 9軸センサBMX055 フライト

総重量　34.84ｇ ESP8266から 9軸 センサ BMX055 を読込 、Madgwickフィルタ・PID制御し、 RemoteXYアプリ（ WiFi）操作による 浮上テストを行いましたが部屋が狭くスマホ操作は難しいです。　   ●同ブログ【 ドローン製作⑭ ESP8266...

  • 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...