HOME | Raspberry Pi | 時事 | ビジネス書籍紹介 | 2022-12-01 (Thu) Today's Access : 106 Total : 559781. Since 10 Sep. 2019

ドローン制御 第2回テレメトリ通信
2022.07.02

YouTube でも紹介しています。画像をクリックすると再生できます。


ドローン制御第2回では、テレメトリーによるドローンと地上局による双方向通信を行います。 プロポが操縦者からドローンへの一方向送信に対して、テレメトリーは双方向通信により、ドローンの機体状態を取得して、ドローンの制御を行います。

■概要図

ブレッドボード上に2つのマイコンボードを配置してしまっていますが、 左側の Adafruit QT Py ESP32S2が地上局、右側のArduino Nano 33 IoT がドローンのフライトコントローラになっていて、 この2つは独立したシステムです。 2つのマイコンボードは、WiFi UDPにより、テレメトリー通信を実現しています。

↑クリックで拡大、再クリックで元に戻ります。

■テレメトリ通信処理概要
今回の大まかな処理の流れは以下の通りです。
・地上局であるQT Py ESP32S2 ではまず、NTP SERVER に接続して時刻情報を取得します。
・QT Py は、WiFi UDP通信により、ドローンのフライトコントローラである Nano33 IoT と時刻同期を行うと同時にドローンの機体状態を取得します。
・QT Py は、Nano 33 IoT に飛行指示を行い、その都度、機体状態を取得します。
Raspberry Pi はソースコードのビルドおよび実行時の送受信をモニタリングします。
第2回では、双方向通信を確立させることが目的なので、各種センサ情報と飛行指示による姿勢制御計算は行いません。

■双方向通信データ構造
ドローンとのテレメトリ通信には、MAVLINK(MicroAir Vehicle Link)と呼ばれる小型無人機と通信するためのプロトコルが使用されています。
このプロトコルに準拠しようとすると実装までに時間が掛かるので、独自のデータ構造を使用します。

typedef struct {
	char          stx;       
	char          sysid[16]; 
	unsigned long epoch_sec; 
	unsigned char seq;       
	char          msgtype[4];
	PAYLOAD       payload;   
	unsigned char checksum;  
} DATA_LINK;

typedef struct {
	char     cmd[3];
	int16_t  opt_x;
	int16_t  opt_y;
	int16_t  opt_z;
	uint16_t speed;
	float    accel_x;
	float    accel_y;
	float    accel_z;
	float    gyro_x;
	float    gyro_y;
	float    gyro_z;
	~
} PAYLOAD;

このデータ構造は暫定的なものです。開発過程で改善していきます。

■Nano 33 IoT IMU(慣性計測装置)3軸加速度/ジャイロセンサ値の取得

まずは、Nano33 IoT に搭載されているIMU LSM6DS3(3軸加速度/ジャイロセンサ)を試してみます。
ソースコードのビルドには、PlatformIOを用います。
Arduino開発環境構築 PlatformIO

$ pio device list
/dev/ttyACM1
------------
Description: QT Py ESP32-S2 - TinyUSB CDC

/dev/ttyACM0
------------
Description: Arduino NANO 33 IoT

/dev/ttyAMA0
------------
Description: ttyAMA0

※Raspberry Pi model 3B に、Nano 33 IoT と QT Py をUSB接続した状態で、接続デバイスを確認しています。
Nano 33 IoT のラズベリーパイへのUSB接続は、/dev/ttyACM0 として認識されています。

$ mkdir ~/Nano33IoT
$ cd ~/Nano33IoT
$ pio boards "nano_33"
Platform: atmelsam
=======================================================
ID          MCU        Frequency Flash RAM  Name
----------- ---------- --------- ----- ---- -----------
nano_33_iot SAMD21G18A 48MHz     256KB 32KB NANO 33 IoT
$ pio init -b nano_33_iot
$ vi platformio.ini
[env:nano_33_iot]
platform = atmelsam
board = nano_33_iot
framework = arduino
 ↓追記
board_build.mcu = samd21g18a
board_build.f_cpu = 48000000L
upload_protocol = sam-ba
upload_port = /dev/ttyACM0

$ pio lib search "header:WiFiNINA.h"
$ pio lib install 5538

【慣性計測ユニット LSM6DS3】
Nano 33 IoTで使用されるLSM6DS3(IMU)は、ジャイロスコープと加速度計の両方として機能します。 ただし、磁力計を含むIMUほど複雑なデバイスではありません。

Ref.慣性センサの基礎知識~ジャイロセンサ、加速度センサ~

◇シンプルな加速度計
#include <SPI.h>
#include <Arduino_LSM6DS3.h>

void setup() {
  Serial.begin(9600);
  while (!Serial);

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Acceleration in G's");
  Serial.println("X\tY\tZ");
}

void loop() {
  float x, y, z;

  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(x, y, z);

    Serial.print(x);
    Serial.print('\t');
    Serial.print(y);
    Serial.print('\t');
    Serial.println(z);
  }
}
$ pio lib search "header:Arduino_LSM6DS3.h"
$ pio lib install 6578
$ pio lib search "header:SPI.h"
$ pio lib install 873
$ pio run -t upload
$ pio device monitor -p /dev/ttyACM0 -b 9600
ccelerometer sample rate = 104.00 Hz

Acceleration in G's
X       Y       Z
0.00    0.01    1.00
0.03    0.02    0.91
0.02    0.00    0.97
0.02    0.00    0.98
0.02    0.00    0.98
ブレッドボードを振ってみると
-0.29   -0.54   0.90
-0.30   -0.61   0.94
-0.29   -0.63   0.95
-0.29   -0.60   0.92
-0.29   -0.49   0.94
-0.28   -0.36   0.96

◇シンプルなジャイロスコープ
#include <Arduino_LSM6DS3.h>

void setup() {
  Serial.begin(9600);
  while (!Serial);

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  Serial.print("Gyroscope sample rate = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Gyroscope in degrees/second");
  Serial.println("X\tY\tZ");
}

void loop() {
  float x, y, z;

  if (IMU.gyroscopeAvailable()) {
    IMU.readGyroscope(x, y, z);

    Serial.print(x);
    Serial.print('\t');
    Serial.print(y);
    Serial.print('\t');
    Serial.println(z);
  }
}
$ pio run -t upload
$ pio device monitor -p /dev/ttyACM0 -b 9600
Gyroscope sample rate = 104.00 Hz

Gyroscope in degrees/second
X       Y       Z
0.24    -3.48   -2.08
0.18    -3.42   -2.08
0.24    -3.36   -2.01
0.31    -3.36   -2.01
0.24    -3.48   -2.01

それでは、Nano 33 IoT と QT Py ESP32S2 とのテレメトリ通信を行ってみます。 まずは地上局側のQT Pyです。

■地上局 QT Py ESP32S2

$ mkdir ~/QTPyESP32S2
$ cd ~/QTPyESP32S2
$ pio init -b adafruit_qtpy_esp32s2
$ vi platformio.ini
[env:adafruit_qtpy_esp32s2]
platform = espressif32
board = adafruit_qtpy_esp32s2
framework = arduino
 ↓追記
platform_packages =
 framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32
build_flags =
 -DBOARD_HAS_PSRAM
 -mfix-esp32-psram-cache-issue
lib_deps =  plerup/EspSoftwareSerial@^6.15.2
upload_port = /dev/ttyACM1

NTPサーバに接続して、現在時刻を取得します。

ソースコード抜粋
#define NTP_SERVER1 "ntp.nict.jp"
#define NTP_SERVER2 "pool.ntp.org"
#define TIMEZONE_JST (3600 * 9)
#define DAYLIGHTOFFSET_JST (0)

static time_t tm_local         = 0;
static unsigned long tm_offset = 0;

void get_epoch(time_t *t, unsigned long *offset) {
  WiFi.begin(SECRET_SSID, SECRET_PASS); // Connect to WPA/WPA2 network
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  configTime(TIMEZONE_JST, DAYLIGHTOFFSET_JST, NTP_SERVER1, NTP_SERVER2);
  for (int cnt=0;cnt<20;++cnt) {
    *t = time(NULL);
    if (*t>TIMEZONE_JST) break;
    delay(500);
  }
  *offset = millis();

  WiFi.disconnect();
  while (WiFi.status() != WL_DISCONNECTED) {
    delay(500);
  }
  for(int i=0; i<20; ++i) {
    delay(500);
  }
}

void setup() {

	get_epoch(&tm_local,&tm_offset);
	~
}
【ポイント】
・NTPサーバから時刻を取得すると同時に、システム起動時からの経過時間を、offset値として保存します。
・テレメトリ通信は、LAN内部での操作としているので、WAN側への接続を切断します。
・切断後、すぐに再接続すると、うまく通信ができないので、10秒ほど待機します。原因がブロードバンドルーターに起因するのか、マイコンボード内の問題なのかは不明です。

飛行指示用のパケット送信部分です。
void sendPacket(){
  strcpy(lnk.sysid, SYSTEM_ID);
  lnk.epoch_sec = tm_local + (millis() - tm_offset)/1000;
  lnk.checksum  = calc_checksum(&lnk);

  while(1) {
    udp.beginPacket(NANO33_IOT, UDP_PORT);
    udp.write((uint8_t*)&lnk,sizeof(DATA_LINK));
    udp.endPacket();

    for (int i=0; i<5; i++) {
      if (receivePacket()) return;
      vTaskDelay(200);
    }
  }
}
【ポイント】
・ドローン側への送信情報に時刻情報を付加し、同期させます。 先に取得した時刻にシステム経過時間を加算し、そこからoffset値を差し引いて、現在時刻を取得しています。
・パケット送信後、ドローン側から応答パケットが返らない、あるいは正常な応答ではない場合には、パケットの再送を行います。

送信パケットへのチェックサムの付与、および応答パケット確認用のチェックサムです。排他的論理和を採用しています。
unsigned char calc_checksum(DATA_LINK *lpt) {
  unsigned char checksum = 0;
  unsigned char *pt = (unsigned char*)lpt;
  while( pt!=&lpt->checksum ) {
    checksum ^= *pt;
    pt++;
  }
  return checksum;
}
【ポイント】
・構造体の末尾に付加した checksum項目直前までの値に対して、排他的論理和を取っています。
・while()部分では、ポインタのアドレスチェックによるループ処理を行っています。 sizeof()演算子を使用したいところですが、sizeof()は、パディング領域も含む構造体型全体のサイズを返します。 つまり、構造体のサイズは全メンバ型サイズの合計以上となってしまいます。

ドローン側からの応答パケットの検証部分です。
bool receivePacket(){

  int packetSize = udp.parsePacket();
  if(packetSize > 0) {
    int len = udp.read((uint8_t*)&res, packetSize);
    if(len == sizeof(DATA_LINK)) {
      if (res.checksum == calc_checksum(&res)) {
        if (memcmp(res.msgtype,"ERR",3) != 0) {
          return true;
        } else {
          return false;
        }
      } else {
        return false;
      }
    } else {
      return false;
    }
  } else {
    return false;
  }
}
【ポイント】
次に該当した場合には、操作指示パケットの再送を行います。
・応答パケットの構造体メンバ msgtype が "ERR" の場合
・チェックサムと不整合がある場合
・パケットサイズが正しくない場合
・応答がない場合

テスト用操作コマンドを送信します。
void setup() {
  get_epoch(&tm_local,&tm_offset);

  WiFi.config(IP_QTPY, DNS, GATEWAY, SUBNETMASK);

  WiFi.begin(SECRET_SSID, SECRET_PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  udp.begin(UDP_PORT);

  memset(&lnk,0x00,sizeof(DATA_LINK));
  lnk.stx = 0xFE;

  lnk.seq = 1;
  strcpy(lnk.msgtype,"HLO");
  sendPacket();

  lnk.seq = 1;
  strcpy(lnk.msgtype,"CMD");
  strcpy(lnk.payload.cmd,"IDL");
  lnk.payload.speed = 5;
  sendPacket();
  vTaskDelay(3000);

  lnk.seq = 1;
  strcpy(lnk.msgtype,"CMD");
  strcpy(lnk.payload.cmd,"UP ");
  lnk.payload.opt_x = 100;
  lnk.payload.speed = 80;
  sendPacket();
  vTaskDelay(3000);

  ~
}
ここでは、流れは以下の通りです。
1. msgtype = "HLO" でドローン側と挨拶を交わします。
2. msgtype = "CMD"、payload.cmd = "IDL" でドローンをアイドリング状態にします。
3. msgtype = "CMD"、payload.cmd = "UP " でドローンを離陸させます。
~以下、続く~

※テレメトリ通信のテストなので、各種オプション指定は無効、ドローン側のモーターも適当に回しています。

■ドローン側フライトコントローラ Nano 33 IoT

次にドローン側の制御です。
void loop() {
  if (receivePacket()) sendPacket((char*)"STA");
  delay(50);
}
ループ処理で地上局からの操作指示パケットを待ち受けます。

受信したパケットは次のように評価されます。
bool receivePacket() {

  int packetSize = udp.parsePacket();
  if(packetSize > 0) {
    int len = udp.read((uint8_t*)&lnk, packetSize);
    if (len == sizeof(DATA_LINK)) {
      if (memcmp(&lnk,&prev,sizeof(DATA_LINK)) != 0) {
        if (lnk.checksum == calc_checksum(&lnk)) {
          tm_local  = lnk.epoch_sec;
          tm_offset = millis();
          memcpy(&prev,&lnk,sizeof(DATA_LINK));

          if (strcmp(lnk.msgtype,"CMD")==0) {
            flight(lnk.payload.cmd, lnk.payload.opt_x, 
                 lnk.payload.opt_y, lnk.payload.opt_z, 
                 lnk.payload.speed);
          }
          return true;
        } else {
          memset(&lnk.payload,0x00,sizeof(PAYLOAD));
          sendPacket((char*)"ERR");
          return false;
        }
      } else {
        return true;
      }
    } else {
      memset(&lnk.payload,0x00,sizeof(PAYLOAD));
      sendPacket((char*)"ERR");
      return false;
    }
  } else {
    return false;
  }
}
【ポイント】
・受信パケットサイズが構造体サイズに等しく、前回受信したパケットとは異なり、チェックサムが正しく、 msgtype が "CMD" の場合に機体制御を行います。 msgtype は "STA" として、機体状態を付与して応答パケットを送信します。 また、受信パケット内の時刻情報に同期します。
・前回受信したパケットと同一の場合は、機体制御は行わず、msgtype を "STA" として、機体状態を付与して応答パケットを送信します。
・チェックサムが正しくない場合には、msgtype に "ERR" を付与して応答パケットを送信します。
・受信パケットサイズが正しくない場合には、msgtype に "ERR" を付与して応答パケットを送信します。

msgtype が "CMD" のときに、PAYLOAD構造体メンバーの cmd の内容に従い機体制御を行います。 今回は適当にPWM信号を出力しているだけです。 機体制御に関しては、次回以降掘り下げていく予定です。
void flight(char *cmd, int16_t x, int16_t y, int16_t z, uint16_t speed) {

  // idling
  if (strcmp(cmd,"IDL")==0) {
    analogWrite(PWM_LF, 10);
    analogWrite(PWM_RR, 10);
    analogWrite(PWM_RF, 10);
    analogWrite(PWM_LR, 15);
  // motor off
  } else if (strcmp(cmd,"MOF")==0) {
    analogWrite(PWM_LF, 0);
    analogWrite(PWM_RR, 0);
    analogWrite(PWM_RF, 0);
    analogWrite(PWM_LR, 0);
  // go advance
  } else if (strcmp(cmd,"FWD")==0) {
    analogWrite(PWM_LF, 20);
    analogWrite(PWM_RR, 40);
    analogWrite(PWM_RF, 20);
    analogWrite(PWM_LR, 40);

  ~
}
エラーの場合を除き、応答パケットには機体状態であるセンサー情報を付加します。
bool sensor() {
  float x, y, z;
  if (IMU.begin()) {
    if (IMU.accelerationAvailable()) {
      IMU.readAcceleration(x, y, z);
      lnk.payload.accel_x = x;
      lnk.payload.accel_y = y;
      lnk.payload.accel_z = z;
    }
    if (IMU.gyroscopeAvailable()) {
      IMU.readGyroscope(x, y, z);
      lnk.payload.gyro_x = x;
      lnk.payload.gyro_y = y;
      lnk.payload.gyro_z = z;
    }
    return true;
  } else {
    return false;
  }
}
■テレメトリ通信テスト
PlatformIOのdevice monitorを基地局とドローン用にそれぞれ立ち上げて、パケット送受信の様子を監視してみます。
QT Py ESP32S2
$ pio device monitor -p /dev/ttyAMA0 -b 115200
Nano 33 IoT
$ pio device monitor -p /dev/ttyACM0 -b 115200



■ソースコードに関して
全ソースコードは、内容がもうちょっと纏まってから掲載する予定です。

■次回以降の予定
実行環境の大枠ができたので、次回以降は、Interface 2020年3月号ドローン&ロボ制御を参考にして詳細に落とし込んでいく予定です。

■参考文献
Dronecode - MAVLinkの入門
MAVLink Basics
espressif/arduino-esp32
Arduino(ESP32)ライブラリリファレンス
ドローンをFPV化したい人のための制作ガイド(開局申請)
 Raspberry Pi(ラズベリー パイ)は、ARMプロセッサを搭載したシングルボードコンピュータ。イギリスのラズベリーパイ財団によって開発されている。
2022.05.27 まずはロボットカーでお勉強
2022.06.10 第1回テスト環境構築
2022.07.02 第2回テレメトリ通信


Interface 2022年12月号
◎実用レベルのデータ・サーバを個人で作れる時代 ☆特集 [AWS/ブロックチェーン/Docker]IoTデータ・サーバ入門
◎モニタリングはもちろん異常検知や画像認識,外部装置駆動まで!


日経Linux 2022年11月号
【特集1】「Ubuntu 22.04 LTS」リリースの今こそ! Windowsを捨てない!Linuxへの引っ越し術
【特集2】話題のChromeOS Flexなど8種を厳選! 軽量Linux TPO別 選択&活用術
【特集3】 Linuxで旧型PCを復活! ファンレスの静音・小型PC「Ubuntu Server 22.04 LTS」でオンラインストレージサーバー化
【特集4】 個人なら無料で使える! Red Hat Enterprise Linux 9を触ってみよう


トランジスタ技術 2022年11月号
★特集◎科学の魅力をとことん追求!計測,パワー応用技術 ☆実験サイエンス!超音波×エレキの世界


ラズパイマガジン2022年秋号
特集:5大人気ボード 電子工作超入門
「半導体不足で在庫が不足し、電子工作のボードがなかなか買えない…」。そんな今にふさわしい特集を企画しました。5種の人気ボードにすべて対応した電子工作の入門特集です。「GPIO」や「I2C」を使った電子パーツの制御方法は、どのボードでも同じです。手に入れられたボードを使って、今こそ電子工作を始めましょう。

地方で稼ぐ! ITエンジニアのすすめ
学歴、理系の知識、専門スキル……全部なくてもITエンジニアになれる! 地方でも高収入でやりがいをもって働ける!ITエンジニアの魅力を一挙大公開

ハッカーの学校 IoTハッキングの教科書
生活にとけこみ、家電機器を便利にするIoT技術。 Webカメラなど、便利の裏側に潜むセキュリティの危険性をハッキングで検証。 専門家がパケットキャプチャからハードウェアハッキングまで、その攻撃と防御を徹底解説。 本書は2018年7月に刊行された「ハッカーの学校IoTハッキングの教科書」に一部修正を加えた第2版です。

攻撃手法を学んで防御せよ! 押さえておくべきIoTハッキング
本書は、経済産業省から2021年4月にリリースされた、IoTセキュリティを対象とした『機器のサイバーセキュリティ確保のためのセキュリティ検証の手引き』の『別冊2 機器メーカに向けた脅威分析及びセキュリティ検証の解説書』をもとに、IoT機器の開発者や品質保証の担当者が、攻撃者の視点に立ってセキュリティ検証を実践するための手法を、事例とともに詳細に解説しました。

ポチらせる文章術
販売サイト・ネット広告・メルマガ・ブログ・ホームページ・SNS… 全WEB媒体で効果バツグン! カリスマコピーライターが教える「見てもらう」「買ってもらう」「共感してもらう」すべてに効くネット文章術

プログラマーは世界をどう見ているのか 西村博之著
イーロン・マスク(テスラ)、ジェフ・べゾス(Amazon)、ラリー・ペイジ(Google)…etc. 世界のトップはなぜプログラマーなのか?

Arduino Nano 33 IoT


サンハヤト モジュール MM-TXS01


ニーア オートマタ PLAY ARTS改 <ヨルハ 二号 B型 DX版> PVC製 塗装済み可動フィギュア
「NieR:Automata」より、ヨルハ二号B型こと2BがPLAY ARTS改に新たに登場! 高級感の感じられるコスチュームや髪の質感、洗練されたボディバランス、細かなデティールに至るまでこだわり抜かれた逸品。 DX版には通常版のラインナップに加え2Bの随行支援ユニット ポッド042などをはじめ“純白の美しい太刀"白の約定やエフェクトパーツ、自爆モードを再現できる換装用ボディパーツ、シーンに合わせて変えられる顔パーツ2種も付属する豪華な仕様に。 作中のあらゆるシーンを再現することが可能なファン必見の一品となっている。

DIPスイッチで動作電圧を3.3Vと5Vに切り替えられるUNO互換ボード
KEYESTUDIO Plus Board for Arduino UNO R3 with Type-C USB Cable, 3.3V 5V 1.5A Output Current, More Powerful Controller Board USB-シリアルチップ:CP2102 / 動作電圧:5Vまたは3.3V(DIPスイッチ制御)/ 外部電源:DC 6-15V(9V推奨)/ デジタルI / Oピン:14(D0〜D13)/ PWMチャネル:6(D3 D5 D6 D9 D10 D11)/ アナログ入力チャネル(ADC):8(A0-A7)/ DC出力機能の各I / Oポート:20 mA / 3.3Vポートの出力能力:50 mA / フラッシュメモリ:32 KB(うち0.5 KBはブートローダーによって使用されます)/ SRAM:2 KB(ATMEGA328P-AU)/ EEPROM:1 KB(ATMEGA328P-AU)/ クロック速度:16MHz / オンボードLEDピン:D13

Newtonライト2.0 ベイズ統計
ベイズ統計は,結果から原因を推定する統計学です。AIや医療などの幅広い分野で応用されています。その基礎となるのは18世紀に考えだされた「ベイズの定理」です。 この本では,ベイズ統計学のきほんをやさしく紹介していきます。

白光(HAKKO) ダイヤル式温度制御はんだ吸取器 ハンディタイプ FR301-81


サンハヤト TTW-203 テストワイヤ
ブレッドボードとスルーホール間の接続に便利なワイヤですブレッドボード用のオスピンと基板のスルーホール用のバネ性のある端子を組み合わせたテストワイヤです

無水エタノールP 500mlx2個パック(掃除)


ケイバ(KEIBA) マイクロニッパー MN-A04


熱収縮チューブφ1.5
印字無しで綺麗☆ シュリンクチューブ 絶縁チューブ 防水 高難燃性 収縮チューブ (2m, ブラック(黒))


サンハヤト SAD-101 ニューブレッドボード


白光(HAKKO) HEXSOL 巻はんだ 精密プリント基板用 150g FS402-02


【Amazon.co.jp限定】エーモン 電工ペンチ 全長約255mm (1452)


[Amazon限定ブランド]【指定第2類医薬品】PHARMA CHOICE 解熱鎮痛薬 解熱鎮痛錠IP 100錠


Copyright © 2011-2022 Sarako Tsukiyono All rights reserved®.