ドローン制御 第２回テレメトリ通信
2022.07.02
YouTube でも紹介しています。画像をクリックすると再生できます。
ドローン制御第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プロセッサを搭載したシングルボードコンピュータ。イギリスのラズベリーパイ財団によって開発されている。
Arduinoで学ぶ組込みシステム入門(第2版)
●Arduinoを使って組込みシステム開発を理解する
・ハードウェアやソフトウェアなどの基礎知識/
・設計から実装までを系統的に説明するモデルベース開発/
・Arduinoを用いた実際の開発例
最新 使える! MATLAB 第3版
◆◆すぐに「使える!」 全ページフルカラー!◆◆
・MATLAB R2022bに対応し、解説もより詳しく!/
・コマンド・スクリプトの例が豊富で、動かして学べる!/
・超基本から解説。これから使いはじめる人にぴったり!/
・全編フルカラー、スクリーンショットも豊富!
Amazon Web Services基礎からのネットワーク＆サーバー構築改訂４版
1.システム構築をインフラから始めるには/
2.ネットワークを構築する/
3.サーバーを構築する/
4.Webサーバーソフトをインストールする/
5.HTTPの動きを確認する/
6.プライベートサブネットを構築する/
7.NATを構築する/
8.DBを用いたブログシステムの構築/
9.TCP/IPによる通信の仕組みを理解する
C言語は第二の母国語: 独学学生時代から企業内IT職人時代に培った、独立のための技術とノウハウ 平田豊著
学生時代から独学でプログラミングをはじめ、企業内でデバイスドライバを開発し、そして独立後もたくさんのアプリケーション開発や技術書制作に携わってきた著者。その筆者が大事に使い続ける「C言語」の“昔と今”について、気づいたことや役立つ知識、使ってきたツールなどについて、これまで記してきたことを整理してまとめました。
本書では、現役プログラマーだけでなく、これからプログラミングを学ぶ学生などにも有益な情報やノウハウを、筆者の経験を元に紹介しています。
1冊ですべて身につくJavaScript入門講座
・最初の一歩が踏み出せる！ 初心者に寄り添うやさしい解説
・最新の技術が身につく！ 今のJavaScriptの書き方・使い方
・絶対に知っておきたい！ アニメーションとイベントの知識
・プログラミングの基本から実装方法まですべて学べる
図解！ Git & GitHubのツボとコツがゼッタイにわかる本
ソフトウェア開発では欠かすことのできないGit、GitHub。
これからGit、GitHubを使いたいという入門者の方でも、実際に手を動かしながら使い方を学べます。
C自作の鉄則！2023 (日経BPパソコンベストムック)
メーカー製のパソコンはスペックが中途半端で、自分が本当に欲しい機種がない――。そう思っている人には、ぜひ自作パソコンをお薦めします。自作パソコンのパーツは進化が速く、しかも驚くほど種類が豊富。価格も性能も、幅広く用意されているため、満足度100％の“自分だけの1台”を手に入れることができます。
Interface 2023年6月号
特集：第1部 フィルタ設計 基礎の基礎/
第2部 係数アプリや波形観測アプリで合点！FIR&IIRフィルタ作り/
第3部 配布プリント基板で体験！マイコンで動くフィルタ作り
日経Linux 2023年5月号
【特集 1】 AI時代の最強フリーソフト ～ 25のやりたいを実現！
【特集 2】 AWS、Azureのうまみを無料で体感！面倒なことはクラウドに任せよう
【特集 3】 新しいRaspberry Pi Cameraで遊んでみよう
【特集 4】 Linuxで旧型PCを復活！ 1kg切るモバイルPCを「ChromeOS Flex」でChromebook化
ラズパイマガジン2022年秋号
特集：5大人気ボード 電子工作超入門
「半導体不足で在庫が不足し、電子工作のボードがなかなか買えない…」。そんな今にふさわしい特集を企画しました。5種の人気ボードにすべて対応した電子工作の入門特集です。「GPIO」や「I2C」を使った電子パーツの制御方法は、どのボードでも同じです。手に入れられたボードを使って、今こそ電子工作を始めましょう。
地方で稼ぐ! ITエンジニアのすすめ
学歴、理系の知識、専門スキル……全部なくてもITエンジニアになれる!
地方でも高収入でやりがいをもって働ける!ITエンジニアの魅力を一挙大公開
Raspberry Piのはじめ方2022
本書は、ラズパイやPicoの買い方やインストール、初期設定といった基本から、サーバー、電子工作、IoT、AIといったラズパイならではの活用方法まで、1冊でお届けします。
ラズパイをこれから始める方向けに、全36ページの入門マンガ「女子高生とラズベリーパイ」も巻末に掲載。これを読むだけでラズパイがどんなものなのか、すぐに分かって触れるようになります。
ハッカーの学校 IoTハッキングの教科書
生活にとけこみ、家電機器を便利にするIoT技術。
Webカメラなど、便利の裏側に潜むセキュリティの危険性をハッキングで検証。
専門家がパケットキャプチャからハードウェアハッキングまで、その攻撃と防御を徹底解説。
本書は2018年7月に刊行された「ハッカーの学校IoTハッキングの教科書」に一部修正を加えた第2版です。
攻撃手法を学んで防御せよ! 押さえておくべきIoTハッキング
本書は、経済産業省から2021年4月にリリースされた、IoTセキュリティを対象とした『機器のサイバーセキュリティ確保のためのセキュリティ検証の手引き』の『別冊2 機器メーカに向けた脅威分析及びセキュリティ検証の解説書』をもとに、IoT機器の開発者や品質保証の担当者が、攻撃者の視点に立ってセキュリティ検証を実践するための手法を、事例とともに詳細に解説しました。
ポチらせる文章術
販売サイト・ネット広告・メルマガ・ブログ・ホームページ・SNS…
全WEB媒体で効果バツグン!
カリスマコピーライターが教える「見てもらう」「買ってもらう」「共感してもらう」すべてに効くネット文章術
プログラマーは世界をどう見ているのか 西村博之著
イーロン・マスク(テスラ)、ジェフ・べゾス(Amazon)、ラリー・ペイジ(Google)…etc.
世界のトップはなぜプログラマーなのか?
ニーア オートマタ PLAY ARTS改 <ヨルハ 二号 B型 DX版> PVC製 塗装済み可動フィギュア
「NieR:Automata」より、ヨルハ二号B型こと2BがPLAY ARTS改に新たに登場!
高級感の感じられるコスチュームや髪の質感、洗練されたボディバランス、細かなデティールに至るまでこだわり抜かれた逸品。
DX版には通常版のラインナップに加え2Bの随行支援ユニット ポッド042などをはじめ“純白の美しい太刀"白の約定やエフェクトパーツ、自爆モードを再現できる換装用ボディパーツ、シーンに合わせて変えられる顔パーツ2種も付属する豪華な仕様に。
作中のあらゆるシーンを再現することが可能なファン必見の一品となっている。
Newtonライト2.0 ベイズ統計
ベイズ統計は,結果から原因を推定する統計学です。AIや医療などの幅広い分野で応用されています。その基礎となるのは18世紀に考えだされた「ベイズの定理」です。
この本では,ベイズ統計学のきほんをやさしく紹介していきます。
白光(HAKKO) ダイヤル式温度制御はんだ吸取器 ハンディタイプ FR301-81
無水エタノールP 500mlx2個パック(掃除)
ケイバ(KEIBA) マイクロニッパー MN-A04
サンハヤト SAD-101 ニューブレッドボード
白光(HAKKO) HEXSOL 巻はんだ 精密プリント基板用 150g FS402-02
[Amazon限定ブランド]【指定第2類医薬品】PHARMA CHOICE 解熱鎮痛薬 解熱鎮痛錠IP 100錠
|
