DJI Mavic3を自律飛行させる方法｜外付センサ編１

フェーズ1：XIAO BLE Sense nRF52840での姿勢検出

まずは機体の軽量化や構成をシンプルにすることを考え、使用するマイコンはIMU（6軸センサ）を標準搭載している Seeed XIAO BLE Sense nRF52840 を選択。事前調査でChatGPTがSense nRF52840なら6軸IMU内蔵してるので別でIMUセンサ用意する必要ないと教えてくれた。

実装の状況

• 内蔵IMUを用いて、ロール・ピッチ・ヨーの姿勢角を算出するコードを実装。
• シリアル通信（有線）でのデータ取得までは非常にスムーズに成功。

Wi-Fiがついていないことに気づく

有線のテストがうまくいったので、Geminiに今度はWi-Fiの無線を使ってPCにデータ送るシンプルなテストプログラムを生成の生成お願いしてプログラムをアップロードするが、何回生成してもらっても一向にコンパイルが通らない。5〜6回繰り返した時に、プログラム冒頭のWi-Fi.hみたなヘッダーファイルのとこでまずエラー出続けてもしやと思って調べたら、XIAO BLE Sense nRF52840がWi-Fi無線対応してないことが判明。
XIAO（ESP32系の基板）は基本Wi-Fi対応しているという大きな勘違いをしていた。

フェーズ2：M5StickC Plusに移行

かつて多摩美メ芸の担当してた授業で使ってたM5StickC Plusが手元にあるのをふと思い出す。画面いらないしXIAOに比べると大きいので本番の実装ではないなと思いつつ、IMU6軸センサを標準搭載しているので、こちらに切り替えて実験。

実験結果：無線での姿勢検出に成功

多摩美の授業で使ってた時、簡単に扱えるように設計されているが故、なんとなく自分にとっては痒いところに手が届かない感触があり、扱いづらいと感じていて、その授業以降使うの敬遠して全く触ってなかったんだけど、使ってみたらとても簡単。LLMのおかげというのもある。
てことで、M5StickC Plusに移行したことで、当初の目的であった「姿勢データをWi-Fi経由でPCへ送る」というタスクはすんなり成功。
以下、Geminiにまるっと生成してもらったプログラム

StickCPlus_IMUwifi.ino

#include <M5StickCPlus.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <OSCMessage.h>

const char* ssid = "48E2449E48E3-2G";
const char* password = "2215086939800";
IPAddress outIp(192, 168, 3, 11);
const unsigned int outPort = 8888;
WiFiUDP Udp;

float pitch, roll, yaw = 0;  // yawは自分で計算して保持
float gyroZ_offset = 0;

void setup() {
  M5.begin();
  M5.Imu.Init();

  // 1. ジャイロのオフセット（静止時のゴミ）を取得
  M5.Lcd.println("STAY STILL...");
  float sumZ = 0;
  for (int i = 0; i < 100; i++) {
    float gx, gy, gz;
    M5.Imu.getGyroData(&gx, &gy, &gz);
    sumZ += gz;
    delay(10);
  }
  gyroZ_offset = sumZ / 100.0;  // 平均をオフセットとする

  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) delay(500);
  M5.Lcd.println("READY");
}

void loop() {
  M5.update();

  // 生データの取得
  float ax, ay, az, gx, gy, gz;
  M5.Imu.getAccelData(&ax, &ay, &az);
  M5.Imu.getGyroData(&gx, &gy, &gz);

  // 2. ドリフト補正ロジック
  float currentGZ = gz - gyroZ_offset;  // オフセットを引く

  // デッドバンド設定：回転速度が±0.5度/秒以下なら「静止」とみなして無視
  if (abs(currentGZ) < 1.0) {
    currentGZ = 0;
  }

  // 3. 積分して角度を算出 (20ms周期なので 0.02を掛ける)
  yaw += currentGZ * 0.02;

  // Pitch/Rollは標準関数から取得（これらは加速度で補正されるので安定しています）
  float p, r, y_unused;
  M5.Imu.getAhrsData(&p, &r, &y_unused);

  // OSC送信
  OSCMessage msg("/m5/imu");
  msg.add(p);    // Pitch
  msg.add(r);    // Roll
  msg.add(yaw);  // 手動補正したYaw

  Udp.beginPacket(outIp, outPort);
  msg.send(Udp);
  Udp.endPacket();
  msg.empty();

  // 4. ボタンA（正面の大きなボタン）でいつでもYawを0にリセット
  if (M5.BtnA.wasPressed()) {
    yaw = 0;
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.println("YAW RESET");
  }

  delay(20);
}

MaxでPitch Roll Yawの値を受け取ってるけど、主にYaw（機体の向いてる方向）が欲しいのでそこの値だけ使ってjitterの赤いキューブの角度に紐づけて簡易的にビジュアライズテスト。

次の課題：センサの精度と拡張性の限界

M5 StickC Plusでとっても簡単に姿勢情報は取れて、あわよくばこいつをそのまま本番実装に使えるかも？と思ったけど、以下の2点の理由で断念

• センサの精度：肝心の6軸センサのyawの値がほっとくと少しずつズレてく（角度の値が0.1ぐらい減算されていってしまう）
• 距離センサを追加する：ここにさらにToF or LiDAR系の距離センサを3つ（前後上下左右方向分）つけるとなると、USBシリアル扱える数や処理速度に限界がありそう

てことで、改めて構成を練り直す

次回につづく