はじめに

ドローンをSpresenseで制御して，HDRカメラを使用して着陸地点を指定して自動で着陸するシステムを作成するはずでした。

謝罪

タイトル，そしてはじめにから分かるように，未完成です。いや，本当は全く問題なく作成できるはずでしたんですよ...?
ドローンの作成に必要なフレームをサークルの3Dプリンターで印刷したらなんと3Dプリンターが壊れてしまってですね...

(ここに見るも無惨な姿になってしまった3Dプリンタの写真を添付)

修理におおよそ1ヶ月かかると言われました。計画の破綻です。

では，外注すれば良いのでは?

はい，尤もでございます。
ではここで庶民の見方，中国の3Dプリントサービスを見てみます。すると，なんということでしょう。中国と日本の関係が何か微妙になっているではありませんか! 当然ドローンのフレームを印刷してくれなんて言えない訳です。え?日本の3Dプリントサービスを使えって?予算が尽きます。

...
という訳で，当初計画をしていた，ドローンをSpresenseで制御して，HDRカメラを使用して着陸地点を指定して自動で着陸するシステムは，時間内に実現不可という形になってまいました。
と，いう訳で，現時点での完成しているモノを提出させていだたくという形になります。なお，当然記事で紹介している方法で，ドローンをSpresenseで制御して，HDRカメラを使用して着陸地点を指定して自動で着陸が出来るかどうかは不明です。

モニター提供品

• SPRESENSE 拡張ボード
• メインボード
• HDR カメラボード

モニター提供をしてくださった方々に大変感謝いたします。

システム構成

仕組み

以下のような順番で動かす予定でした。

1. ドローンを離陸させる。
2. ドローンを北向きに前進させる。
3. 地面に置いてあるArucマーカーをカメラからの映像で認識させる。
4. Arucoマーカーの検知位置にドローンを着地させる。

ハードウェア

フレーム

ドローンの骨格となるフレームをCADで作成しました。

プログラム

当然完成はしていませんし，このプログラムをそのまま書き込んでも動きません。(まだ各部の連携が取れていないので...)

ドローン制御部

ドローンが動くかどうかのテスト
#include <Arduino.h>

#include "betafpvdef.h"
#include "sendMpsp.h"

unsigned long stateStartTime = 0;

void setup()
{
  Serial.begin(115200);  // USBデバッグ用 (PCのシリアルモニタ)
  Serial2.begin(115200); // FCとの通信用 (D0/D1ピン)

  Serial.println("MSP Control Start!");

  // 初期化
  rcData[CH_ROLL] = 1500;
  rcData[CH_PITCH] = 1500;
  rcData[CH_THROTTLE] = 1000;
  rcData[CH_YAW] = 1500;
  rcData[CH_AUX1] = 1000;
}

void loop()
{
  unsigned long currentTime = millis();
  unsigned long timeInState = currentTime - stateStartTime;

  switch (currentState)
  {
  case STATE_WAIT_START:
    // 起動後5秒間は待機
    if (timeInState >= 5000)
    {
      currentState = STATE_ARMING;
      stateStartTime = currentTime;
      Serial.println("State: ARMING");
    }
    break;
  case STATE_ARMING:
    // アームON、スロットルはアイドリング
    rcData[CH_AUX1] = 2000;         // アームON
    rcData[CH_THROTTLE] = THR_IDLE; // アイドリング

    if (timeInState >= 3000)
    {
      currentState = STATE_TAKEOFF;
      stateStartTime = currentTime;
      Serial.println("State: TAKEOFF");
    }
    break;

  case STATE_TAKEOFF:
    // スロットルをホバリングまで上げる。
    rcData[CH_AUX1] = 2000; // アームON
    rcData[CH_THROTTLE] = THR_HOVER;

    // 2秒間上昇or浮上 をする
    if (timeInState >= 2000)
    {
      // 終わったら離陸する。とりあえず
      currentState = STATE_LANDING;
      stateStartTime = currentTime;
      Serial.println("State: HOVER");
    }
    break;

  case STATE_LANDING:
    rcData[CH_AUX1] = 2000;         // アームON
    rcData[CH_THROTTLE] = THR_LAND; // 着

    // 3秒間降下する
    if (timeInState >= 3000)
    {
      currentState = STATE_DISARM;
      stateStartTime = currentTime;
      Serial.println("State: DISARM");
    }

    break;

  case STATE_DISARM:
    // モータ停止
    rcData[CH_AUX1] = 1000;        // アームOFF
    rcData[CH_THROTTLE] = THR_MIN; // スロットル停止

    Serial.println("State: FINISHED");
    currentState = STATE_FINISHED;
    break;

  case STATE_FINISHED:
    // 何もしない(一応停止信号を出し続ける)
    rcData[CH_AUX1] = 1000;        // アームOFF
    rcData[CH_THROTTLE] = THR_MIN; // スロット
    break;
  }

  // RCデータ送信
  sendRcData();

  delay(20); // 50Hz送信
}

画像処理部

Arucoマーカーを検出し，その重心を算出します。(重心の位置によってドローンがどの方向に動くかどうかを計算することが出来ます)

ArucoLite認識
#include <Arduino.h>
#include <Camera.h>

// ArucoLiteの設定
#define ARUCO_DB ARUCO_DB_4X4_1000

#define ARUCO_DB_SIZE 100
#include <ArucoLite.h>

ArucoLite<320, 240, false> Aruco;

void setup()
{
  Serial.begin(115200);

  // カメラの初期化
  theCamera.begin(
      1,
      CAM_VIDEO_FPS_30,
      CAM_IMGSIZE_QVGA_H,
      CAM_IMGSIZE_QVGA_V,
      CAM_IMAGE_PIX_FMT_YUV422,
      7);

  theCamera.startStreaming(true, CamCB);

  Serial.println("Aruco detection started!");
}

void loop()
{
}

// カメラから画像が届くたびに呼ばれる関数
void CamCB(CamImage img)
{

  if (!img.isAvailable())
    return;

  memcpy(Aruco.frame, img.getImgBuff(), img.getImgSize());

  Aruco.process();

  if (Aruco.arucos_found > 0)
  {
    Serial.print("Found: ");
    Serial.println(Aruco.arucos_found);

    for (int i = 0; i < Aruco.arucos_found; i++)
    {
      aruco_t &detected_marker = Aruco.result[i];

      // IDの表示
      Serial.print("  [ID: ");
      Serial.print(detected_marker.aruco_idx);
      Serial.print("] Center: (");

      // 重心（中心）の計算
      // コーナー4点の平均を取る
      float cx = 0, cy = 0;
      for (int p = 0; p < 4; p++)
      {
        cx += detected_marker.pt[p].x;
        cy += detected_marker.pt[p].y;
      }
      cx /= 4.0;
      cy /= 4.0;

      Serial.print(cx);
      Serial.print(", ");
      Serial.print(cy);
      Serial.println(")");
    }
    Serial.println("---");
  }
}

最後に

完成することが出来なかったのはやはり3Dプリンターが故障したことにあります。3Dプリンターが修理された暁には何とかして完成させたいと思います(ハードウェア買うのになかなかのお金をつぎ込んだので無駄にしたくない)。