ドローン制御 第２回テレメトリ通信
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月号ドローン&ロボ制御を参考にして詳細に落とし込んでいく予定です。
■参考文献
・Arduino Nano 33 IoT Board - Getting Started
・Dronecode - MAVLinkの入門
・MAVLink Basics
・espressif/arduino-esp32
・Arduino(ESP32)ライブラリリファレンス
・ドローンをFPV化したい人のための制作ガイド(開局申請)
|
Raspberry Pi（ラズベリー パイ）は、ARMプロセッサを搭載したシングルボードコンピュータ。イギリスのラズベリーパイ財団によって開発されている。
たいていのことは100日あれば、うまくいく。長田英知著
「時間がなくて、なかなか自分のやりたいことができない」
「一念発起して何かを始めても、いつも三日坊主で終わってしまう」
「色んなことを先延ばしにしたまま、時間だけが過ぎていく」
そこで本書では、そんな著者が独自に開発した、
まったく新しい目標達成メソッド「100日デザイン」について、
その知識と技術を、余すところなくご紹介します。
まんがで納得ナポレオン・ヒル 思考は現実化する
OLとして雑務をこなす日々に飽き足らず、科学者だった父が残した薬品を商品化すべく、起業を決意した内山麻由(27)。彼女はセミナーで知り合った謎の女性からサポートを得ながら、彼女と二人三脚でナポレオン・ヒルの成功哲学を実践し、さまざまな問題を乗り越えていく。
ヒル博士の<ゴールデンルール>に従い、仕事に、恋に全力疾走する彼女の、成功への物語。
今日は人生最悪で最高の日 １秒で世界を変えるたったひとつの方法 ひすいこたろう著
偉人の伝記を読むと、最悪な日は、不幸な日ではなく、新しい自分が始まる日であることがわかります。最悪な出来事は、自分の人生が、想像を超えて面白くなる兆しなのです。偉人伝を読むことで、このときの不幸があったおかげで、未来にこういう幸せがくるのかと、人生を俯瞰する視線が立ち上がるのです。
ご飯は私を裏切らない heisoku著
辛い現実から目を背けて食べるご飯は、いつも美味しく幸せを届けてくれる。
29歳、中卒、恋人いない歴イコール年齢。バイト以外の職歴もなく、短期バイトを転々とする日々。ぐるぐると思索に耽るけど、ご飯を食べると幸せになれる。奇才の新鋭・heisokuが贈るリアル労働グルメ物語!
【最新版Gemini 3に対応！】できるGemini (できるシリーズ)
Geminiを「最強の知的生産パートナー」として使いこなすための、実践的なノウハウを凝縮した一冊です。
基本的な操作方法から、具体的なビジネスシーンでの活用、日々の業務を自動化するGoogle Workspaceとの連携、さらには自分だけのオリジナルAIを作成する方法まで余すところなく解説します。
Rustプログラミング完全ガイド 他言語との比較で違いが分かる!
Rustの各手法や考え方を幅広く解説！
500以上のサンプルを掲載。実行結果も確認。
全24章の包括的なチュートリアル。
ポチらせる文章術
販売サイト・ネット広告・メルマガ・ブログ・ホームページ・SNS…
全WEB媒体で効果バツグン!
カリスマコピーライターが教える「見てもらう」「買ってもらう」「共感してもらう」すべてに効くネット文章術
小型で便利な Type-C アダプター USB C オス - USB3.1 オスアダプター
Type-C端子のマイコンボードをこのアダプタを介して直接Raspberry Piに挿すことができます。ケーブルなしで便利なツールです。
Divoom Ditoo Pro ワイヤレススピーカー
15W高音質重低音/青軸キーボード/Bluetooth5.3/ピクセルアート 専用アプリ/USB接続/microSDカード
電源供給USBケーブル スリム 【5本セット】
USB電源ケーブル 5V DC電源供給ケーブル スリム 【5本セット】 電源供給 バッテリー 修理 自作 DIY 電子工作 (100cm)
|
