コメント無し

# はじめに 　SPRESENSEでドローンを作ったので報告連絡相談。 　目標は下記の通り。 ミニマムサクセス　：ホバリング フルサクセス　　　：高さ制御 エキストラサクセス：マーカーで位置制御 　なお、下記のモニター提供品をモニター提供していただいた。 提供品１　Spresense メインボード 　　　　・用途　主演算装置 提供品２　Spresense 拡張ボード 　　　　・用途　IO拡張 提供品３　Spresense カメラボード（通常カメラ） 　　　　・用途　マーカー検出用 提供品４　Wi-SUN モジュール 　　　　・用途　各種センサ値取得用 　上記提供品の使用箇所を下記図に示す。  # システム構成 　以下にシステム構成を示す。  　下記にドローンの外観を示す。  　Wi-SUNモジュールがカメラケーブルと干渉することに組み立てて初めて気が付いた。ドローンの外観下部に示すようにカメラケーブルを90度折り曲げてWi-SUNモジュールを装着している。 　以下に配線図を示す。  　提供品含めたパーツリストを下記に示す。 1. Spresense メインボード 1. Spresense 拡張ボード 1. Spresense カメラボード（通常カメラ） 1. Spresense Wi-SUN モジュール 1. ブラシ付きDCモータ(Tello互換品) 1. プロペラ(Tello互換品) 1. 加速度・ジャイロセンサ(BMI160) 1. 距離センサ(GPY2Y0E03) 1. Lipoバッテリ 1s

1. RB160M-30TR(ショットキーバリアダイオード) 1. AP7333(3.3Vレギュレータ) 1. AO3400A(Nch-FET) 1. チップ抵抗(10kΩと100Ω) 1. フレーム(3Dプリンタ出力) 1. モータードライバ用基板(FusionPCBに作成依頼)

# ソフトウェア構成 　以下にソフトウェア構成を示す。  　SPRESENSEはマルチコアプログラミングが可能なので各種機能（ソフトウェア）をMain CoreとSub Coreに配置する。 　下記に機能配置図を示す。  　機能配置にはハードウェアの制約がかかわってくる。Arduino IDEで開発するとき、カメラ関係はMain Coreでしか操作できないためカメラの前処理と位置推定はMain Coreに配置してそれ以外はSub Coreに配置している。 # 機能の説明 - 受信・送信処理 　コマンドの送受信にはTWELITE を使用しており、Spresenseとはソフトウェアシリアルで通信している。データは電源投入されてから経過した時間2byte、ステータス1byte、2byteごとに分割された-1~1までのfloat型のパラメータ12個、スタートバイト2byte、エンドバイト2byteの合計31byte。 　実機デバッグ時のモニタ用のデータはWi-SUNモジュールから受信しており、電源投入されてから経過した時間2byte、2byteごとに分割された-1~1までのfloat型のパラメータ4個の合計10byte。 　なんとなく操縦系とその他で通信方式を分けてみた。片方通信途絶したとき縮退完了まで操縦できるなどのフェールセーフ的用途では有効かも。 下記に受信・送信の概念図を示す。  - 画像取得 　画像は最終的にグレースケールを経由して二値化処理するため、グレースケール化が容易なCAM_IMAGE_PIX_FMT_YUV422フォーマットで取得した。 　YUV422は1pixel当たり2byteの情報量を持っており、その2byte中のビッグエンディアンで左から3bit~11bit目にかけてを取り出すと8bit分解能の輝度データが抽出できる。  - 前処理 　まず取得したグレースケール画像を二値化し領域ごとにラベリングを行う。その後、各領域に対して矩形と仮定したときの頂点を検出する。最後に矩形かどうかを判別するフィルタをかけてマーカー（黒い矩形）を検知する。 　ラベリング処理に関しては「C言語による画像処理プログラミング入門」という緑と黒の表紙の怪しい書籍を参考にした。サンプルプログラムを使用して解説しているので非常に分かりやすかった。この書籍を出版していた昭晃堂はすでに解散したとのことだが、同著者の似たような本が朝倉書店から出版されていた。おそらく同じような内容かと思われる。 　頂点検出は下記図の通り、各領域の水平垂直の最大最小値の４点と画像を45度回転させたときの４点を比較し、対角位置の頂点距離v、hとv45、h45で最も大きいほうを選択するようにした。  　矩形判別には縦横比、充填率、面積を閾値に設定し、範囲外の領域は除外して最終的に残った領域が矩形であるというような方針をとっている。 @[twitter](https://twitter.com/k218675554/status/1561651788251803649) - 位置推定 　透視投影変換によって姿勢位置を計算したいところだが、計算負荷などの問題でできなかったため簡易的な手法で代用した。 　マーカーから得られる矩形の４つの頂点から、方向、水平距離、前後距離を算出した。方向は矩形の垂直方向の２辺の比によって算出し、水平距離は矩形中心の水平方向pixel位置とし、前後距離は矩形の面積とした。 　ドローン水平方向移動と水平距離取得データの様子を下記に示す。データはWi-SUNモジュールで取得している。 @[twitter](https://twitter.com/k218675554/status/1572219802852429824) 　ドローンヨー方向回転と方向取得データの様子を下記に示す。 @[twitter](https://twitter.com/k218675554/status/1572219617703235585) - センサ値取得 　センサーは加速度、角速度（ジャイロ）を有するBMI160、距離センサのGPY2Y0E03の２種類を使用した。 BMI160の使用に関して下記のライブラリのお世話になった。 [BMI160-Arduino](https://github.com/hanyazou/BMI160-Arduino) - 姿勢推定 　姿勢推定は、ドローン制御によく用いられているMadgwickフィルタを使用した。 　下記ライブラリのお世話になった。 　[MadgwickAHRS](https://github.com/arduino-libraries/MadgwickAHRS ) - 高度推定 　センサーはドローン底部に固定するため、姿勢が傾くと地面からの距離が大きく計測されそのままでは制御に使用できない。センサ取り付け位置も影響することに注意が必要である。推定した姿勢角と距離センサの計測値からドローンと地面からの高度を推定した。 - 制御量算出 　角速度を制御するインナーループと姿勢角を制御するアウターループの階層構造を有するPD制御とした。 　高さ制御は、単純なPD制御にドローン重量分の推力オフセットを与えた。積分値を制御に使いたくない派なのでこのような実装になっているが、単純にPID制御で実装したほうが性能はいいかもしれない。 　位置制御に関しては水平、前後、方向指示値を姿勢角にゲインをかけた指示値として与えている。単純なP制御のみ。マーカー水平距離計測値と目標位置の偏差をヨー角、マーカー前後距離計測値と目標位置の偏差をピッチ角、マーカー方向計測値と目標方位の偏差をロール角の指示値にしている。 　制御周期は50Hzとしている。 - PWM出力 　算出した制御量に基づいてPWM出力を行う。Arduino IDEを使用する場合、標準ライブラリのanalogwriteは8bit分解能のduty比しか操作できないが、[本ページ](https://elchika.com/article/ce38a60e-2b6a-4992-a217-33d060ccef0d/)のようにして高分解能のPWM出力を使用することができる。 # その他補足事項 　操縦系については下記のようなソフトをPythonで作った。GUIが簡単に作れるので便利。 @[twitter](https://twitter.com/k218675554/status/1558530272811171840) 　操作モードは下記の三種類 1. 制御モード 　制御を実施する。スロットル、ロール、ピッチ、ヨーの操作はジョイパッドで行う。GUI上のスライダでゲインを調整できる。 1. モータ動作確認モード 　GUI上のスライダの操作によってモータが回転する。スライダ1~4がモータ1~4に対応している。 1. キャリブレーションモード 　ジャイロセンサのバイアス除去とMadgwickフィルタのクォータニオンリセットを実施する。 # まとめ 　実際に動作している様子を下記に示す。 　このようにフルサクセスの高さ制御は達成できたが、エキストラサクセスのマーカーによる制御はイマイチうまくいっていない。 　下記に高さ制御で飛行しているドローンの様子を示す。 @[twitter](https://twitter.com/k218675554/status/1571006690123993092) 　下記にマーカーによる定位置制御を試みているドローンの様子を示す。 ※スロットルON or OFFの指示のみ与えている。 @[twitter](https://twitter.com/k218675554/status/1572787614620749824) @[twitter](https://twitter.com/k218675554/status/1572787730777780227) @[twitter](https://twitter.com/k218675554/status/1573604292694798336) @[twitter](https://twitter.com/k218675554/status/1573604421426376705) 　ドローンはマーカーの方向を向くが水平・前後方向にはふらふらしてしまう。ゲイン上げ過ぎるとハンチングするし小さすぎると追従しない。 　うまく定位置制御できないのは、単純にホバリングの安定性が悪くカメラ画角からマーカーが外れてしまうことや、またマーカー方向計測値の粗さなどが原因と考えられる。 　ホバリングの安定性については、ゲインチューニングを繰り返してもこれが限界のようだった。これまでに同じスケールのドローンを作成したことがあり、制御周期がホバリングの安定性に効くことを経験した。100Hzでは安定したホバリングが実現できたが50Hzではイマイチだった。今回制御周期が50Hzなのは単純に100Hzのサイクルでは処理が追い付かなかったため。処理の軽量化は頑張ればできるのかもしれないが、時間の都合上妥協してしまった。 　マーカー方位計測値については、矩形のサイズが計測値分解能に与える影響がでかいことが考えられる。カメラで取得している画像解像度が96x64で、映った矩形のサイズは一辺当たり20~10pixel程度と小さい。マーカーに対してカメラが10度程度傾いてやっと計測値が動くくらいのなので非常に粗い計測しかできていない。かといって解像度を増やすと更新周期が遅くなるのでなかなか難しい。 　今後、処理の軽量化を進めて安定したホバリングの実現を目指すとともに、マーカーによる位置制御もしっかり実現させたい。 　ドローンの制御と画像処理が同時・リアルタイムで走るのは凄い。Spresenseのパワフルさを実感した。 # ソースコード 　ソースコードを下記に示す。 ## SubCore1 ### 受信処理 ```arduino:com_sub1.ino //for sub core #include <MP.h> #include <SoftwareSerial.h> SoftwareSerial mySerial(7, 8); // RX,TXの割り当て #define MSGLEN 31 #define MY_MSGID 10 #define Si1buf_size 1024 #define Si1readbyte_len MSGLEN struct SPacket { volatile int status; /* 0:ready, 1:busy */ int statuspc = 0; float pc_t = 0.0; float paramdata[12]; }; struct RPacket { volatile int status; /* 0:ready, 1:busy */ int statusmpu = 0; float mpu_t = 0.0; float reg_data[12]; }; char Serial1_buffer[Si1buf_size]; int Serial1_buffer_len = 0; int statuspc = 0; float pc_t = 0.0; int statusmpu = 0; float mpu_t = 0.0; float reg_data[12]; #define SUBID "Sub1" SPacket packet; void setup() { memset(&packet, 0, sizeof(packet)); MP.begin(); mySerial.begin(57600); /* Polling */ MP.RecvTimeout(MP_RECV_POLLING); } void loop() { R_L(); W_L(); } void W_L() { //データ送信する関数 RPacket *rpacket; int subid; int8_t msgid; subid = 2; /* Receive message from SubCore */ int ret = MP.Recv(&msgid, &rpacket, subid); if (ret > 0) { statusmpu = rpacket->statusmpu; mpu_t = rpacket->mpu_t; reg_data[0] = rpacket->reg_data[0]; reg_data[1] = rpacket->reg_data[1]; reg_data[2] = rpacket->reg_data[2]; reg_data[3] = rpacket->reg_data[3]; reg_data[4] = rpacket->reg_data[4]; reg_data[5] = rpacket->reg_data[5]; reg_data[6] = rpacket->reg_data[6]; reg_data[7] = rpacket->reg_data[7]; reg_data[8] = rpacket->reg_data[8]; reg_data[9] = rpacket->reg_data[9]; reg_data[10] = rpacket->reg_data[10]; reg_data[11] = rpacket->reg_data[11]; /* status -> ready */ rpacket->status = 0; int i = 0; byte s_uchMsg[31]; word ushTmp; //start byte s_uchMsg[0] = 0xa5; s_uchMsg[1] = 0x5a; //statusmpu ushTmp = (byte)(statusmpu); s_uchMsg[2] = ushTmp; //mpu_t ushTmp = (word)(mpu_t / 600.0 * 65535.0); s_uchMsg[3] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[4] = (byte)(ushTmp & 0x00ff); //euler_data ushTmp = (word)((reg_data[0] + M_PI) / (2 * M_PI) * 65535.0); s_uchMsg[5] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[6] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[1] + M_PI) / (2 * M_PI) * 65535.0); s_uchMsg[7] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[8] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[2] + M_PI) / (2 * M_PI) * 65535.0); s_uchMsg[9] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[10] = (byte)(ushTmp & 0x00ff); //abs press data ushTmp = (word)((reg_data[3] + 0.0) / 2000.0 * 65535.0); s_uchMsg[11] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[12] = (byte)(ushTmp & 0x00ff); //com_data ushTmp = (word)((reg_data[4] + 1.0) / 2.0 * 65535.0); s_uchMsg[13] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[14] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[5] + 1.0) / 2.0 * 65535.0); s_uchMsg[15] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[16] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[6] + 1.0) / 2.0 * 65535.0); s_uchMsg[17] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[18] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[7] + 1.0) / 2.0 * 65535.0); s_uchMsg[19] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[20] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[8] + 1.0) / 2.0 * 65535.0); s_uchMsg[21] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[22] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[9] + 1.0) / 2.0 * 65535.0); s_uchMsg[23] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[24] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[10] + 1.0) / 2.0 * 65535.0); s_uchMsg[25] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[26] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[11] + 1.0) / 2.0 * 65535.0); s_uchMsg[27] = (byte)(ushTmp >> 8 & 0x00ff); s_uchMsg[28] = (byte)(ushTmp & 0x00ff); //end byte s_uchMsg[29] = 0x0d; s_uchMsg[30] = 0x0a; mySerial.write(s_uchMsg, sizeof(s_uchMsg)); } } void R_L() { int subid; subid = 2; float get_data[16]; int i = 0; int tmp_len = 0; char Serial1_buffer_tmp[Si1buf_size]; for (i = 0; i < Si1buf_size; i++) { Serial1_buffer_tmp[i] = 0x00; } //シリアル情報取得 while ( mySerial.available()) { if (tmp_len >= Si1buf_size)break; Serial1_buffer_tmp[tmp_len] = mySerial.read(); tmp_len = tmp_len + 1; } //バッファに貯める if (tmp_len > (Si1buf_size - Serial1_buffer_len)) { //バッファサイズからはみ出るとき、はみ出た分削除の後、取得分追加 // Serial.println("バッファサイズからはみ出るとき、はみ出た分削除の後、取得分追加"); int over_len = 0; over_len = tmp_len - (Si1buf_size - Serial1_buffer_len); char swp_sil1_buf[Si1buf_size]; for (i = 0; i < Si1buf_size; i++) { swp_sil1_buf[i] = Serial1_buffer[i]; } for (i = 0; i < Si1buf_size - over_len; i++) { Serial1_buffer[i] = swp_sil1_buf[i + over_len]; } Serial1_buffer_len = Serial1_buffer_len - over_len; for (i = 0; i < tmp_len; i++) { Serial1_buffer[Serial1_buffer_len + i] = Serial1_buffer_tmp[i]; } Serial1_buffer_len = Serial1_buffer_len + tmp_len; // Serial.println(Serial1_buffer_len); } else { //バッファサイズからはみ出ないとき、取得分追加 // Serial.println("バッファサイズからはみ出ないとき、取得分追加"); for (i = 0; i < tmp_len; i++) { Serial1_buffer[Serial1_buffer_len + i] = Serial1_buffer_tmp[i]; } Serial1_buffer_len = Serial1_buffer_len + tmp_len; // Serial.println(Serial1_buffer_len); } //データ読み出し for (int i = 0; i < Serial1_buffer_len - Si1readbyte_len; i++) { int buf_len = 0; if ( Serial1_buffer[i + buf_len] == 0xa5 ) { buf_len = buf_len + 1; if ( Serial1_buffer[i + buf_len] == 0x5b ) { buf_len = buf_len + 1; int n = 0; word ushTmp; char low; char high; //statuspc statuspc = (int)Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; //pc_t low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); pc_t = ((double)ushTmp / 65535.0 * 600.0); //param1 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[0] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param2 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[1] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param3 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[2] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param4 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[3] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param5 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[4] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param6 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[5] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param7 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[6] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param8 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[7] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param9 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[8] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param10 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[9] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param11 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[10] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); //param12 low = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; high = Serial1_buffer[i + buf_len]; buf_len = buf_len + 1; ushTmp = (word)(((low << 8) & 0xff00) | (high & 0x00ff)); get_data[11] = ((double)ushTmp / 65535.0 * 2.0 - 1.0); if ( Serial1_buffer[i + buf_len] == 0x0d ) { buf_len = buf_len + 1; if ( Serial1_buffer[i + buf_len] == 0x0a ) { buf_len = buf_len + 1; //パケット取得成功のため、取得分より前データ削除 int over_len = i + buf_len; char swp_sil1_buf[Si1buf_size]; for (int i = 0; i < Si1buf_size; i++)swp_sil1_buf[i] = Serial1_buffer[i]; for (int i = 0; i < Si1buf_size - over_len; i++)Serial1_buffer[i] = swp_sil1_buf[i + over_len]; Serial1_buffer_len = Serial1_buffer_len - over_len; packet.statuspc = statuspc; packet.pc_t = pc_t; for (int j = 0; j < 12; j++) { packet.paramdata[j] = get_data[j]; } if (packet.status == 0) { /* status -> busy */ packet.status = 1; MP.Send(MY_MSGID, &packet, subid); } } } } } } } ``` ## MainCore ### 画像取得＆前処理＆位置推定 ```arduino:drone_004.ino #include <MP.h> #include <SDHCI.h> #include <stdio.h> #include <Camera.h> #define M_PI 3.1415927 #define M_GR 9.80665 #define M_R 0.0034837 #define M_T 273.15 #define M_P0 1013.25 #define MOT0 9 #define MOT1 6 #define MOT2 5 #define MOT3 3 #define MY_MSGID 11 #define image_h (96) #define image_v (64) struct MPacket { volatile int status; /* 0:ready, 1:busy */ int statusmain = 0; float main_t = 0.0; float paramdata[12]; }; MPacket packet; int cnt2 = 0;//データ送信カウンタ float t = 0.0; float dt = 0.001;//s boolean cnt2_flag = LOW; float cnt2_time = 0.02;//割り込み周期[sec]//データ送信カウンタ int statusmain = 0; float paramdata[12]; float reg_data[12]; bool f_timer = 0; bool f_cam = 0; int posc[4][2]; uint16_t image_size = 0; double th_aspe = 3.0; int th_area = 20; int th_lmin = 5; double th_fill_err = 0.1; float rangle45deg = 0.70710678; float image_rot45table[image_h][image_v][2]; uint8_t image_data[image_h * image_v]; uint8_t image[image_h][image_v]; uint8_t* image_buf; int label[image_h][image_v]; void setup() { pinMode(MOT0, OUTPUT); pinMode(MOT1, OUTPUT); pinMode(MOT2, OUTPUT); pinMode(MOT3, OUTPUT); digitalWrite(MOT0, LOW); digitalWrite(MOT1, LOW); digitalWrite(MOT2, LOW); digitalWrite(MOT3, LOW); Serial.begin(115200); delay(200); Serial.println("Serial init end"); IntervalSet(dt); Serial.println("timer init end"); int ret = 0; int subid; memset(&packet, 0, sizeof(packet)); /* Boot SubCore */ subid = 1; ret = MP.begin(subid); if (ret < 0) { printf("MP.begin(%d) error = %d\n", subid, ret); } subid = 2; ret = MP.begin(subid); if (ret < 0) { printf("MP.begin(%d) error = %d\n", subid, ret); } subid = 3; ret = MP.begin(subid); if (ret < 0) { printf("MP.begin(%d) error = %d\n", subid, ret); } /* Polling */ MP.RecvTimeout(MP_RECV_POLLING); Serial.println("Prepare camera"); CamErr err; err = theCamera.begin(1, CAM_VIDEO_FPS_120, image_h, image_v, CAM_IMAGE_PIX_FMT_YUV422 ); if (err != CAM_ERR_SUCCESS) { printError(err); } err = theCamera.startStreaming(true, CamCB); if (err != CAM_ERR_SUCCESS) { printError(err); } affin_rot45(); posc[0][0] = 0; posc[0][1] = 0; posc[1][0] = 0; posc[1][1] = 0; posc[2][0] = 0; posc[2][1] = 0; posc[3][0] = 0; posc[3][1] = 0; } void IntervalSet(float interupt_time) { attachTimerInterrupt(timer_1, uint64_t(interupt_time * 1000000)); } void loop() { if (cnt2_flag) { int subid; subid = 2;//subcore2 packet.statusmain = statusmain; packet.main_t = t; gray_scale(image_buf, image_size); binarize(80); image_copy(); filterling_using_shape(); float right_len = sqrt((float)(posc[0][0] - posc[1][0]) * (posc[0][0] - posc[1][0]) + (float)(posc[0][1] - posc[1][1]) * (posc[0][1] - posc[1][1])); float left_len = sqrt((float)(posc[2][0] - posc[3][0]) * (posc[2][0] - posc[3][0]) + (float)(posc[2][1] - posc[3][1]) * (posc[2][1] - posc[3][1])); float side_angle = 0; float rect_size = (right_len + left_len) * 0.5; float pos_x = (float)(posc[0][0] + posc[1][0] + posc[2][0] + posc[3][0]) / 2.0 / (float)image_h - 1.0; float pos_y = (float)(posc[0][1] + posc[1][1] + posc[2][1] + posc[3][1]) / 2.0 / (float)image_v - 1.0; rect_size = rect_size * rect_size / 4000.0; if (right_len > left_len) { side_angle = right_len / left_len - 1; } else { side_angle = -(left_len / right_len - 1); } reg_data[0] = side_angle; reg_data[1] = rect_size; reg_data[2] = pos_x; reg_data[3] = pos_y; for (int j = 0; j < 12; j++) { packet.paramdata[j] = reg_data[j]; } if (packet.status == 0) { /* status -> busy */ packet.status = 1; MP.Send(MY_MSGID, &packet, subid); } cnt2_flag = LOW; } } uint64_t timer_1() {// t = t + dt; cnt2 = cnt2 + 1; if (cnt2 > (int)(cnt2_time / dt) - 1) { //send data flag cnt2_flag = HIGH; cnt2 = 0; } return uint64_t(dt * 1000000); } void CamCB(CamImage img) { if (img.isAvailable()) { image_buf = img.getImgBuff(); image_size = img.getImgSize(); f_cam = 1; } else { } } void gray_scale(uint8_t* img, uint16_t img_size) { int i, j, k; uint8_t lb = 0; uint8_t hb = 0; word tmp = 0; uint8_t ypix = 0; for (i = 0; i < sizeof(image_data); i++) { lb = img[i * 2]; hb = img[i * 2 + 1]; tmp = ((hb << 8) | lb); ypix = ((tmp >> 11) & 0b11111) << 3; image_data[i] = ypix; } } void binarize(uint8_t t) { int i; /* 画像を２値化 */ for (i = 0; i < sizeof(image_data); i++) { if (image_data[i] > t) { image_data[i] = 255; } else { image_data[i] = 0; } } } void image_copy() { int x, y, cnt; cnt = 0; for (y = 0; y < image_v; y++) { for (x = 0; x < image_h; x++) { image[x][y] = image_data[cnt]; cnt = cnt + 1; } } } void filterling_using_shape() { int i, j, k, x, y, number; int xmin, xmax, ymin, ymax, area; float xminr, xmaxr, yminr, ymaxr; int pos0[4][2], pos1[4][2], pos2[4][2]; double w, h, ratio, fill_rate; double w2, h2; statusmain = 0; number = labeling(); if ( number > 0 ) { if (number > 10) { number = 10; } for (i = 1; i <= number; i++) { xmin = image_h - 1; ymin = image_v - 1; xmax = 0; ymax = 0; xminr = image_rot45table[image_h - 1][0][0]; yminr = image_rot45table[image_h - 1][image_v - 1][1]; xmaxr = image_rot45table[0][image_v - 1][0]; ymaxr = image_rot45table[0][0][1]; for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = 0; pos1[j][k] = 0; pos2[j][k] = 0; } } for (y = 0; y < image_v; y++) for (x = 0; x < image_h; x++) if (label[x][y] == i) { if (x < xmin) { xmin = x; pos1[0][0] = x; pos1[0][1] = y; } if (x > xmax) { xmax = x; pos1[1][0] = x; pos1[1][1] = y; } if (y < ymin) { ymin = y; pos1[2][0] = x; pos1[2][1] = y; } if (y > ymax) { ymax = y; pos1[3][0] = x; pos1[3][1] = y; } if (image_rot45table[x][y][0] < xminr) { xminr = image_rot45table[x][y][0]; pos2[0][0] = x; pos2[0][1] = y; } if (image_rot45table[x][y][0] > xmaxr) { xmaxr = image_rot45table[x][y][0]; pos2[1][0] = x; pos2[1][1] = y; } if (image_rot45table[x][y][1] < yminr) { yminr = image_rot45table[x][y][1]; pos2[2][0] = x; pos2[2][1] = y; } if (image_rot45table[x][y][1] > ymaxr) { ymaxr = image_rot45table[x][y][1]; pos2[3][0] = x; pos2[3][1] = y; } } /* 縦横比（>=1.0)を求める */ /* w/h と h/w の大きい方を採用している */ w = xmax - xmin + 1.0; h = ymax - ymin + 1.0; w2 = xmaxr - xminr + 1.0; h2 = ymaxr - yminr + 1.0; if ( w < h ) { ratio = h / w; if ( w2 < h2 ) { if ( h2 > h ) { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos2[j][k]; } } } else { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos1[j][k]; } } } } else { if ( w2 > h ) { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos2[j][k]; } } } else { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos1[j][k]; } } } } } else { ratio = w / h; if ( w2 < h2 ) { if ( h2 > w ) { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos2[j][k]; } } } else { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos1[j][k]; } } } } else { if ( w2 > w ) { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos2[j][k]; } } } else { for (j = 0; j < 4; j++) { for (k = 0; k < 2; k++) { pos0[j][k] = pos1[j][k]; } } } } } /* ラベル i の面積を求める */ area = 0; for (y = 0; y < image_v; y++) { for (x = 0; x < image_h; x++) { if (label[x][y] == i) { area++; } } } //頂点から面積算出 float area_p = 0.0; area_p = 0.5 * (pos0[0][0] - pos0[2][0]) * (pos0[0][1] + pos0[2][1]); area_p = area_p + 0.5 * (pos0[2][0] - pos0[1][0]) * (pos0[2][1] + pos0[1][1]); area_p = area_p + 0.5 * (pos0[1][0] - pos0[3][0]) * (pos0[1][1] + pos0[3][1]); area_p = area_p + 0.5 * (pos0[3][0] - pos0[0][0]) * (pos0[3][1] + pos0[0][1]); area_p = abs(area_p); //充填率 fill_rate = area / area_p; int lmin = image_h * image_h; for (j = 0; j < 4; j++) { for (k = 0; k < 4; k++) { int lsqrt = (pos0[j][0] - pos0[k][0]) * (pos0[j][0] - pos0[k][0]) + (pos0[j][1] - pos0[k][1]) * (pos0[j][1] - pos0[k][1]); if (j != k) { if (lsqrt < lmin) { lmin = lsqrt; } } } } lmin = sqrt(lmin); if ( ratio > th_aspe || area <= th_area || abs(1.0 - fill_rate) > th_fill_err || lmin < th_lmin) { } else { posc[0][0] = pos0[0][0]; posc[0][1] = pos0[0][1]; posc[1][0] = pos0[1][0]; posc[1][1] = pos0[1][1]; posc[2][0] = pos0[2][0]; posc[2][1] = pos0[2][1]; posc[3][0] = pos0[3][0]; posc[3][1] = pos0[3][1]; /* 数値を昇順にソート */ int tmpx; int tmpy; for (j = 0; j < 4; ++j) { for (k = j + 1; k < 4; ++k) { if (posc[j][0] > posc[k][0]) { tmpx = posc[j][0]; tmpy = posc[j][1]; posc[j][0] = posc[k][0]; posc[j][1] = posc[k][1]; posc[k][0] = tmpx; posc[k][1] = tmpy; } } } if (posc[0][1] > posc[1][1]) { tmpx = posc[1][0]; tmpy = posc[1][1]; posc[1][0] = posc[0][0]; posc[1][1] = posc[0][1]; posc[0][0] = tmpx; posc[0][1] = tmpy; } if (posc[3][1] > posc[2][1]) { tmpx = posc[2][0]; tmpy = posc[2][1]; posc[2][0] = posc[3][0]; posc[2][1] = posc[3][1]; posc[3][0] = tmpx; posc[3][1] = tmpy; } statusmain = 1; break; } } } } void affin_rot45() { int x, y; for (y = 0; y < image_v; y++) { for (x = 0; x < image_h; x++) { image_rot45table[x][y][0] = (float)x * rangle45deg - (float)y * rangle45deg; image_rot45table[x][y][1] = (float)x * rangle45deg + (float)y * rangle45deg; } } } void modify_label( int num1, int num2) { int x, y; for (y = 0; y < image_v; y++) for (x = 0; x < image_h; x++) if ( label[x][y] == num1) label[x][y] = num2; } int search_4neighbors( int x, int y ) { int max = 0; if (y - 1 >= 0 && label[x][y - 1] > max ) max = label[x][y - 1]; /* 上 */ if (x - 1 >= 0 && label[x - 1][y] > max ) max = label[x - 1][y]; /* 左 */ if (y + 1 < image_v && label[x][y + 1] > max ) max = label[x][y + 1]; /* 下 */ if (x + 1 < image_h && label[x + 1][y] > max ) max = label[x + 1][y]; /* 右 */ return max; } int labeling() { int x, y, num; int count = 0; int new_count; for (y = 0; y < image_v; y++) for (x = 0; x < image_h; x++) label[x][y] = 0; for (y = 0; y < image_v; y++) for (x = 0; x < image_h; x++) { if ( image[x][y] == 0 && label[x][y] == 0 ) { num = search_4neighbors(x, y); if (num == 0) label[x][y] = ++count; else label[x][y] = num; } } if ( count > 0 ) { for (y = 0; y < image_v; y++) for (x = 0; x < image_h; x++) if (label[x][y] != 0) { num = search_4neighbors(x, y); if ( num > label[x][y]) { modify_label(num, label[x][y]); } } new_count = 0; for (y = 0; y < image_v; y++) for (x = 0; x < image_h; x++) if ( label[x][y] > new_count ) { new_count++; modify_label(label[x][y], new_count); } return new_count; } else return 0; } void printError(enum CamErr err) { Serial.print("Error: "); switch (err) { case CAM_ERR_NO_DEVICE: Serial.println("No Device"); break; case CAM_ERR_ILLEGAL_DEVERR: Serial.println("Illegal device error"); break; case CAM_ERR_ALREADY_INITIALIZED: Serial.println("Already initialized"); break; case CAM_ERR_NOT_INITIALIZED: Serial.println("Not initialized"); break; case CAM_ERR_NOT_STILL_INITIALIZED: Serial.println("Still picture not initialized"); break; case CAM_ERR_CANT_CREATE_THREAD: Serial.println("Failed to create thread"); break; case CAM_ERR_INVALID_PARAM: Serial.println("Invalid parameter"); break; case CAM_ERR_NO_MEMORY: Serial.println("No memory"); break; case CAM_ERR_USR_INUSED: Serial.println("Buffer already in use"); break; case CAM_ERR_NOT_PERMITTED: Serial.println("Operation not permitted"); break; default: break; } } ``` ## SubCore2 ### センサ値取得（高度・姿勢）＆姿勢・高度推定＆制御量算出＆PWM出力＆送信処理 ```arduino:drone_sub2.ino //for sub core #include <Wire.h> #include <BMI160Gen.h> #include <MP.h> #include <sys/ioctl.h> #include <fcntl.h> #include <nuttx/timers/pwm.h> #include <MadgwickAHRS.h> #define M_PI 3.1415927 #define M_GR 9.80665 #define M_R 0.0034837 #define M_T 273.15 #define M_P0 1013.25 #define SUBID "Sub2" #define MOT0 9 #define MOT1 6 #define MOT2 5 #define MOT3 3 #define s_ulim 1.0 #define s_llim 0.0 //BMI160デバイスアドレス #define i2c_addr 0x68 //距離センサデバイスアドレス #define dist_addr 0x40 #define MY_MSGID 11 //main coreからの受信 struct MPacket { volatile int status; /* 0:ready, 1:busy */ int statusmain = 0; float main_t = 0.0; float paramdata[12]; }; //sub core1からの受信 struct RPacket { volatile int status; /* 0:ready, 1:busy */ int statuspc = 0; float pc_t = 0.0; float paramdata[12]; }; //sub core1に送信 struct SPacket { volatile int status; /* 0:ready, 1:busy */ int statusmpu = 0; float mpu_t = 0.0; float reg_data[12]; }; //sub core3に送信 struct WPacket { volatile int status; /* 0:ready, 1:busy */ int statusmpu = 0; float mpu_t = 0.0; float reg_data[12]; }; struct pwm_state_s { bool initialized; FAR char *devpath; uint8_t duty; uint32_t freq; }; static struct pwm_state_s g_pwmstate; SPacket packet; WPacket packetw; Madgwick filter; unsigned long cnt0 = 0;//時間カウンタ uint16_t cnt1 = 0;//LEDカウンタ uint16_t cnt2 = 0;//データ送信カウンタ uint16_t cnt3 = 0;//制御カウンタ uint16_t cnt4 = 0;//計測カウンタ float t = 0.0; unsigned long time_u = 0; unsigned long pretime_u = 0; float dt = 0.01;//s boolean cnt1_flag = HIGH; boolean cnt2_flag = LOW; boolean cnt3_flag = LOW; boolean cnt4_flag = LOW; float delay_tau = 0.0;//計測制定待ち時間[ms] float cnt1_time = 1.0;//割り込み周期[sec]//LEDカウンタ float cnt2_time = 0.05;//割り込み周期[sec]//データ送信カウンタ float cnt3_time = 0.02;//割り込み周期[sec]//制御カウンタ float cnt4_time = 0.02;//割り込み周期[sec]//計測カウンタ double con_time = 0.0; int statuspc = 0; int statusmain = 0; float pc_t = 0.0; float mpum_t = 0.0; float paramdata[12]; float mparamdata[12]; int statusmpu = 0; float reg_data[12]; float hight_lpfcoef = 1.0; float hight_data = 0.0; float hight_datal = 0.0; float acc_data[3] = {0, 0, 0}; //[m/s^2] float gyro_data[3] = {0, 0, 0}; //[rad/s] float euler_data[3] = {0, 0, 0}; //[rad] float angvel_data[3] = {0, 0, 0}; //[rad/s] float angvel_data_f[3] = {0, 0, 0}; //[rad/s] float rpyv_data[3] = {0, 0, 0}; //[rad/s] float gyro_data_l[3] = {0, 0, 0}; //[rad/s] float acc_data_l[3] = {0, 0, 0}; //[m/s^2] int calib_num = 1000; float acc_offset_data[3] = {0, 0, 0}; float gyro_offset_data[3] = {0, 0, 0}; float euler_offset_data[3] = { -0.8 * M_PI / 180.0, 2.5 * M_PI / 180.0, 0}; float imu_factor = 1.0; float acc_roll = 0.0; float acc_pitch = 0.0; float tau = 1.0 / (2.0 * M_PI * 100); float lpf_ca = 0.0; float lpf_cg = 0.0; float lpf_ch = 0.9; float lpf_data_f[6] = {0, 0, 0, 0, 0, 0}; float pos_gain[3] = {0.0, 0.0, 0.0}; float pos_controllout[3] = {0.0, 0.0, 0.0}; float ang_gain[3] = {0.0, 0.0, 0.0}; float ang_controllout[3] = {0.0, 0.0, 0.0}; float angvel_gain[9] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; float angvel_P[3] = {0.0, 0.0, 0.0}; float angvel_I[3] = {0.0, 0.0, 0.0}; float angvel_D[3] = {0.0, 0.0, 0.0}; float angvel_last[3] = {0.0, 0.0, 0.0}; float angvel_lpf[3] = {0.0, 0.0, 0.0}; float angvel_lpfcoeff = 0.9; float angvel_controllout[3] = {0.0, 0.0, 0.0}; float hight_gain[3] = {0.0, 0.0, 0.0}; float hight_P = 0.0; float hight_I = 0.0; float hight_D = 0.0; float hight_last = 0.0; float hight_controllout = 0.0; float th_zero = 0.55; float pos_data[3] = {0, 0, 0}; //[-] float side_angle = 0.0; float front_dist = 0.0; float h_dist = 0.0; float v_dist = 0.0; float com_mpu[4] = {0, 0, 0, 0}; //[-] void setup() { MP.begin(); mot_init(); printf("Mot init end\n"); sensor_init(); printf("sensor init end\n"); IntervalSet(dt); printf("timer init end\n"); int ret = 0; int subid; memset(&packet, 0, sizeof(packet)); memset(&packetw, 0, sizeof(packetw)); MP.RecvTimeout(MP_RECV_POLLING); } void IntervalSet(float interupt_time) { attachTimerInterrupt(timer_1, uint64_t(interupt_time * 1000000)); } void loop() { ST_CONTROLL(); D_L(); C_L(); W_L(); R_L(); } void ST_CONTROLL() {//状態遷移 if (statusmpu == 0) {//初期状態のとき //初期化中に遷移 statusmpu = 1; } else if (statusmpu == 1) { //初期化中のとき //初期化終了まで待機 初期化処理の中で待機中に遷移 // statusmpu = 1; } else {//待機中　もしくはactxのとき if (statuspc == 3) {//pcのステータスがact0指示のとき //act0に遷移 act0:通常制御 statusmpu = 3; } else if (statuspc == 4) {//pcのステータスがact1指示のとき //act1に遷移 act1:モータ任意操作 statusmpu = 4; } else if (statuspc == 5) { //pcのステータスがact2指示のとき //act2に遷移 act2:キャリブレーション statusmpu = 5; } } } uint64_t timer_1() {//スレッド管理 t = t + dt; cnt0 = cnt0 + 1; cnt1 = cnt1 + 1; cnt2 = cnt2 + 1; cnt3 = cnt3 + 1; cnt4 = cnt4 + 1; // Serial.println(t); if (cnt1 > (uint16_t)(cnt1_time / dt) - 1) { //led function cnt1_flag = !cnt1_flag; cnt1 = 0; } if (cnt2 > (uint16_t)(cnt2_time / dt) - 1) { //send data flag cnt2_flag = HIGH; cnt2 = 0; } if (cnt3 > (uint16_t)(cnt3_time / dt) - 1) { //control flag cnt3_flag = HIGH; cnt3 = 0; } if (cnt4 > (uint16_t)(cnt4_time / dt) - 1) { //check power flag cnt4_flag = HIGH; cnt4 = 0; } return uint64_t(dt * 1000000); } void W_L() { //データ送信 if (cnt2_flag) { int subid; subid = 1; packet.statusmpu = statusmpu; packet.mpu_t = t; packetw.statusmpu = statusmpu; packetw.mpu_t = t; for (int j = 0; j < 12; j++) { packet.reg_data[j] = reg_data[j]; packetw.reg_data[j] = reg_data[j]; } if (packet.status == 0) { /* status -> busy */ packet.status = 1; MP.Send(MY_MSGID, &packet, subid); } subid = 3; if (packetw.status == 0) { /* status -> busy */ packetw.status = 1; MP.Send(MY_MSGID, &packetw, subid); } cnt2_flag = LOW; } } void C_L() { //制御関数 if (cnt3_flag) { control_main(); cnt3_flag = LOW; } } void D_L() { //計測する関数 if (cnt4_flag) { get_GP2Y0E0(); if (statusmpu == 5) {//act0 offset_update(); madgwick_init(); } else { accel_gyro_update(); calc_attitude_Madgwick(); } reg_data[0] = euler_data[0]; reg_data[1] = euler_data[1]; reg_data[2] = euler_data[2]; reg_data[3] = 0.0; reg_data[4] = mparamdata[0]; reg_data[5] = mparamdata[1]; reg_data[6] = mparamdata[2]; reg_data[7] = mparamdata[3]; reg_data[8] = com_mpu[0]; reg_data[9] = com_mpu[1]; reg_data[10] = com_mpu[2]; reg_data[11] = com_mpu[3]; cnt4_flag = LOW; } } void R_L() {//データ受信 RPacket *rpacket; MPacket *mpacket; int subid; int8_t msgid; subid = 1; int ret = MP.Recv(&msgid, &rpacket, subid); if (ret > 0) { statuspc = rpacket->statuspc; pc_t = rpacket->pc_t; paramdata[0] = rpacket->paramdata[0]; paramdata[1] = rpacket->paramdata[1]; paramdata[2] = rpacket->paramdata[2]; paramdata[3] = rpacket->paramdata[3]; paramdata[4] = rpacket->paramdata[4]; paramdata[5] = rpacket->paramdata[5]; paramdata[6] = rpacket->paramdata[6]; paramdata[7] = rpacket->paramdata[7]; paramdata[8] = rpacket->paramdata[8]; paramdata[9] = rpacket->paramdata[9]; paramdata[10] = rpacket->paramdata[10]; paramdata[11] = rpacket->paramdata[11]; rpacket->status = 0; } ret = MP.Recv(&msgid, &mpacket); if (ret > 0) { statusmain = mpacket->statusmain; mpum_t = mpacket->main_t; mparamdata[0] = mpacket->paramdata[0]; mparamdata[1] = mpacket->paramdata[1]; mparamdata[2] = mpacket->paramdata[2]; mparamdata[3] = mpacket->paramdata[3]; mparamdata[4] = mpacket->paramdata[4]; mparamdata[5] = mpacket->paramdata[5]; mparamdata[6] = mpacket->paramdata[6]; mparamdata[7] = mpacket->paramdata[7]; mparamdata[8] = mpacket->paramdata[8]; mparamdata[9] = mpacket->paramdata[9]; mparamdata[10] = mpacket->paramdata[10]; mparamdata[11] = mpacket->paramdata[11]; mpacket->status = 0; } } void control_main(void) {//制御の状態管理 if (statusmpu == 3) {//act0 control_com(); } else if (statusmpu == 4) {//act1 mot0_drive(paramdata[0]); mot1_drive(paramdata[1]); mot2_drive(paramdata[2]); mot3_drive(paramdata[3]); } else { mot0_drive(0.0); mot1_drive(0.0); mot2_drive(0.0); mot3_drive(0.0); } } void control_com(void) {//制御量算出 float Pgain = 0.001 * paramdata[0] + 0.001 * 0.9; float Igain = 0.0; float Dgain = 0.0001 * paramdata[2] + 0.0001 * 2.4; float YPgain = 0.01 * 1.34; float YIgain = 0.0; float YDgain = 0.0; angvel_gain[0] = Pgain; angvel_gain[1] = Igain; angvel_gain[2] = Dgain; angvel_gain[3] = Pgain; angvel_gain[4] = Igain; angvel_gain[5] = Dgain; angvel_gain[6] = YPgain; angvel_gain[7] = YIgain; angvel_gain[8] = YDgain; ang_gain[0] = 1000.0 * paramdata[6] + 1000 * 1.0; ang_gain[1] = 1000.0 * paramdata[7] + 1000 * 1.0; pos_gain[0] = 0.0 * paramdata[1]; pos_gain[1] = 0.0 * paramdata[3]; pos_gain[2] = 0.0; pos_data[0] = front_dist; pos_data[1] = side_angle; pos_data[2] = h_dist; pos_controll(0.3, 0); pos_controll(0.0, 1); pos_controll(0.0, 2); ang_controll((paramdata[8] + 0) * 10.0 / 180.0 * M_PI, 0); //pitch ang_controll((paramdata[9] + 0) * 10.0 / 180.0 * M_PI, 1); //roll // ang_controll(0.0, 2);//yaw angvel_controll(ang_controllout[0] + pos_controllout[0], 0); //pitch angvel_controll(ang_controllout[1] + pos_controllout[0], 1); //rollh // angvel_controll(ang_controllout[2], 2);//yaw angvel_controll(paramdata[10] * 100.0 + pos_controllout[2], 2); //yaw hight_gain[0] = 2.0; hight_gain[1] = 0; hight_gain[2] = 1.0; hight_controll(paramdata[11] - v_dist * paramdata[4]); float throt = hight_controllout; float roll_offset = -0.08; float pitch_offset = 0.346 + paramdata[5]; //mot0 com_mpu[0] = roll_offset - angvel_controllout[1] - pitch_offset + angvel_controllout[0] - angvel_controllout[2] + throt; //mot1 com_mpu[1] = -roll_offset + angvel_controllout[1] - pitch_offset + angvel_controllout[0] + angvel_controllout[2] + throt; //mot2 com_mpu[2] = roll_offset - angvel_controllout[1] + pitch_offset - angvel_controllout[0] + angvel_controllout[2] + throt; //mot3 com_mpu[3] = -roll_offset + angvel_controllout[1] + pitch_offset - angvel_controllout[0] - angvel_controllout[2] + throt; if (statusmpu == 7) { com_mpu[0] = 0.0; com_mpu[1] = 0.0; com_mpu[2] = 0.0; com_mpu[3] = 0.0; } com_mpu[0] = limitter(com_mpu[0], -1.0, 1.0); com_mpu[1] = limitter(com_mpu[1], -1.0, 1.0); com_mpu[2] = limitter(com_mpu[2], -1.0, 1.0); com_mpu[3] = limitter(com_mpu[3], -1.0, 1.0); mot0_drive(com_mpu[0] * 1.0); mot1_drive(com_mpu[1] * 1.0); mot2_drive(com_mpu[2] * 1.0); mot3_drive(com_mpu[3] * 1.0); } void pos_controll(float ref_ang, int target) {//target:0→roll 1→pitch 2→yaw//位置制御 float err = ref_ang - pos_data[target]; pos_controllout[target] = pos_gain[target] * err; } void ang_controll(float ref_ang, int target) {//target:0→roll 1→pitch 2→yaw//角度制御 float err = ref_ang - euler_data[target]; ang_controllout[target] = 2.0 * ang_gain[target] * err; } void angvel_controll(float ref_vel, int target) {//target:0→roll 1→pitch 2→yaw//角速度制御 float err = ref_vel - rpyv_data[target]; float Kpi = 1.0; float Kd = 1.0; angvel_P[target] = Kpi * err; angvel_I[target] = angvel_I[target] + angvel_P[target]; angvel_lpf[target] = angvel_lpf[target] * (1.0 - angvel_lpfcoeff) + Kd * gyro_data[target] * angvel_lpfcoeff; angvel_D[target] = (angvel_lpf[target] - angvel_last[target]) / cnt3_time; angvel_last[target] = angvel_lpf[target]; angvel_I[target] = limitter(angvel_I[target], -1.0, 1.0); angvel_controllout[target] = angvel_gain[target * 3 + 0] * angvel_P[target] + angvel_gain[target * 3 + 1] * angvel_I[target] - angvel_gain[target * 3 + 2] * angvel_D[target]; angvel_controllout[target] = limitter(angvel_controllout[target], -1.0, 1.0); } void hight_controll(float ref_vel) {//高度制御 float err = ref_vel - hight_data; float Kp = 1.0; float Kd = 1.0; hight_P = Kp * err; hight_I = hight_I + hight_P; hight_D = (-hight_data + hight_last) / cnt3_time; hight_last = hight_data; hight_I = limitter(hight_I, -1.0, 1.0); hight_controllout = hight_gain[0] * hight_P + hight_gain[1] * hight_I + hight_gain[2] * hight_D + th_zero; hight_controllout = limitter(hight_controllout, -1.0, 1.0); } float limitter(float input, float lowerlim, float upperlim) {//上限下限リミッタ float output = input; if (input < lowerlim) { output = lowerlim; } if (input > upperlim) { output = upperlim; } return output; } void madgwick_init() {//姿勢推定初期化 filter.begin(50); } void calc_attitude_Madgwick(void) {//姿勢推定 filter.updateIMU(gyro_data[0], gyro_data[1], gyro_data[2], acc_data[0], acc_data[1], acc_data[2]); euler_data[0] = filter.getRollRadians(); euler_data[1] = filter.getPitchRadians(); euler_data[2] = filter.getYawRadians(); float sr = sinf(euler_data[0]); float cr = cosf(euler_data[0]); float sp = sinf(euler_data[1]); float cp = cosf(euler_data[1]); rpyv_data[0] = gyro_data[0] + sr * sp / cp * gyro_data[1] + cr * sp / cp * gyro_data[2]; rpyv_data[1] = 0.0 + cr * gyro_data[1] - sr * gyro_data[2]; rpyv_data[2] = 0.0 + sr * 1.0 / cp * gyro_data[1] + cr * 1.0 / cp * gyro_data[2]; } void calc_pos(void) {//簡易位置推定 side_angle = mparamdata[0] * 1000 / 150; if (side_angle > 1.0) { side_angle = 1.0; } if (side_angle < -1.0) { side_angle = -1.0; } front_dist = 2.5 * (sqrt(mparamdata[1]) - 0.4); if (front_dist > 1.0) { front_dist = 1.0; } if (front_dist < 0.0) { front_dist = 0.0; } h_dist = mparamdata[2]; v_dist = mparamdata[3]; } void mot_init(void) {//モータ初期化 pwm_init(2000, 0.0); mot0_drive(0.0); mot1_drive(0.0); mot2_drive(0.0); mot3_drive(0.0); } void mot0_drive(float _mot) {//モータ0 PWM出力 _mot = _mot - 0.0; if (_mot > s_ulim) { _mot = s_ulim; } if (_mot < s_llim) { _mot = s_llim; } pwm_output(3 , 2000, _mot); } void mot1_drive(float _mot) {//モータ1 PWM出力 _mot = _mot - 0.0; if (_mot > s_ulim) { _mot = s_ulim; } if (_mot < s_llim) { _mot = s_llim; } pwm_output(1 , 2000, _mot); } void mot2_drive(float _mot) {//モータ2 PWM出力 _mot = _mot - 0.0; if (_mot > s_ulim) { _mot = s_ulim; } if (_mot < s_llim) { _mot = s_llim; } pwm_output(0 , 2000, _mot); } void mot3_drive(float _mot) {//モータ3 PWM出力 _mot = _mot - 0.0; if (_mot > s_ulim) { _mot = s_ulim; } if (_mot < s_llim) { _mot = s_llim; } pwm_output(2 , 2000, _mot); } void pwm_output(int pin_num, int freq, float duty) {//PWM出力処理 int fd; int ret; struct pwm_info_s info; char* devpath; if (pin_num == 0) { devpath = "/dev/pwm0\0"; } else if (pin_num == 1) { devpath = "/dev/pwm1\0"; } else if (pin_num == 2) { devpath = "/dev/pwm2\0"; } else if (pin_num == 3) { devpath = "/dev/pwm3\0"; } else { devpath = "/dev/pwm0\0"; } if (g_pwmstate.devpath) { free(g_pwmstate.devpath); } g_pwmstate.devpath = strdup(devpath); fd = open(g_pwmstate.devpath, O_RDONLY); if (fd < 0) { close(fd); return; } info.frequency = freq; info.duty = (uint32_t)(duty * 65535); ret = ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info)); if (ret < 0) { close(fd); return; } ret = ioctl(fd, PWMIOC_START, 0); if (ret < 0) { close(fd); return; } close(fd); } void pwm_init(int freq, float duty) {//PWM出力処理初期化 if (!g_pwmstate.initialized) { g_pwmstate.duty = (uint32_t)(duty * 65535); g_pwmstate.freq = freq; g_pwmstate.initialized = true; } } void sensor_init(void) {//センサ初期化 cnt2_flag = HIGH; statusmpu = 1;//初期化中 W_L();//状態送信 Wire.begin(); //加速度ジャイロセンサ初期化 BMI160.begin(BMI160GenClass::I2C_MODE, i2c_addr); //offset for (int i = 0; i < 1000; i++) { int gix, giy, giz; int aix, aiy, aiz; BMI160.readGyro(gix, giy, giz); BMI160.readAccelerometer(aix, aiy, aiz); gyro_offset_data[0] = gyro_offset_data[0] + convertRawGyro(gix); gyro_offset_data[1] = gyro_offset_data[1] + convertRawGyro(giy); gyro_offset_data[2] = gyro_offset_data[2] + convertRawGyro(giz); } gyro_offset_data[0] = gyro_offset_data[0] / 1000.0; gyro_offset_data[1] = gyro_offset_data[1] / 1000.0; gyro_offset_data[2] = gyro_offset_data[2] / 1000.0; cnt2_flag = HIGH; statusmpu = 2;//待機中 W_L(); } void offset_update() {//バイアス更新 //offset gyro_offset_data[0] = 0.0; gyro_offset_data[1] = 0.0; gyro_offset_data[2] = 0.0; for (int i = 0; i < 1000; i++) { int gix, giy, giz; int aix, aiy, aiz; BMI160.readGyro(gix, giy, giz); BMI160.readAccelerometer(aix, aiy, aiz); gyro_offset_data[0] = gyro_offset_data[0] + convertRawGyro(gix); gyro_offset_data[1] = gyro_offset_data[1] + convertRawGyro(giy); gyro_offset_data[2] = gyro_offset_data[2] + convertRawGyro(giz); } gyro_offset_data[0] = gyro_offset_data[0] / 1000.0; gyro_offset_data[1] = gyro_offset_data[1] / 1000.0; gyro_offset_data[2] = gyro_offset_data[2] / 1000.0; } void accel_gyro_update() {//加速度・角速度更新 int gix, giy, giz; int aix, aiy, aiz; BMI160.readGyro(gix, giy, giz); BMI160.readAccelerometer(aix, aiy, aiz); float acc_c[3]; float gyro_c[3]; acc_c[0] = convertRawAcceleration(aix) - acc_offset_data[0]; acc_c[1] = convertRawAcceleration(aiy) - acc_offset_data[1]; acc_c[2] = convertRawAcceleration(aiz) - acc_offset_data[2]; acc_data[0] = (1.0 - lpf_ca) * acc_c[0] + lpf_ca * acc_data_l[0]; acc_data[1] = (1.0 - lpf_ca) * acc_c[1] + lpf_ca * acc_data_l[1]; acc_data[2] = (1.0 - lpf_ca) * acc_c[2] + lpf_ca * acc_data_l[2]; gyro_c[0] = convertRawGyro(gix) - gyro_offset_data[0]; gyro_c[1] = convertRawGyro(giy) - gyro_offset_data[1]; gyro_c[2] = convertRawGyro(giz) - gyro_offset_data[2]; gyro_data[0] = (1.0 - lpf_cg) * gyro_c[0] + lpf_cg * gyro_data_l[0]; gyro_data[1] = (1.0 - lpf_cg) * gyro_c[1] + lpf_cg * gyro_data_l[1]; gyro_data[2] = (1.0 - lpf_cg) * gyro_c[2] + lpf_cg * gyro_data_l[2]; } float convertRawAcceleration(int aRaw) {//加速度変換 float a = (aRaw * 2.0) / 32768.0; return a; } float convertRawGyro(int gRaw) {//角速度変換 float g = (gRaw * 250.0) / 32768.0; return g; } void get_GP2Y0E0() {//高度取得 uint8_t DEV_ADDR = dist_addr; Wire.beginTransmission(DEV_ADDR); Wire.write(0x5E); Wire.endTransmission(false); int len = Wire.requestFrom(DEV_ADDR, 2, true); uint8_t c[2] ; if (Wire.available()) { c[0] = Wire.read() ; // データの11-4ビット目を読み出す c[1] = Wire.read() ; } float buf_data = (((float)c[0] * 16.0 + (float)c[1]) / 16.0) / 4.0 * 1.000; if (buf_data < 3.0 || buf_data > 63.0) { buf_data = 3.0; } float offset_l = 0.044;//センサ取り付け位置オフセット分 float hight_datac = offset_l * (sinf(euler_data[1]) + sinf(euler_data[0])) + buf_data / 100.0f * cosf(euler_data[1]) * cosf(euler_data[0]); hight_data = (1.0 - lpf_ch) * hight_datac + lpf_ch * hight_datal; hight_datal = hight_data; if (hight_data > 0.6) { hight_data = 0.0; } if (hight_data < 0.027) { hight_data = 0.027; } } ``` ## SubCore3 ### 送信処理（Wi-SUN） ```arduino:wisun_sub3.ino #include <MP.h> #include "bp35c0-j11.h" #define SUBID "Sub3" #define MSGLEN 31 #define MY_MSGID 10 unsigned char state = 0 ; int statusmpu = 0; float mpu_t = 0.0; float reg_data[12]; float dt = 0.05;//s boolean timer_flg = false; BP35C0J11 bp35c0j11; struct RPacket { volatile int status; /* 0:ready, 1:busy */ int statusmpu = 0; float mpu_t = 0.0; float reg_data[12]; }; void setup() { MP.begin(); MP.RecvTimeout(MP_RECV_POLLING); boolean rc = FALSE ; bp35c0j11.j11_init(); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 1 ; // hardware reset end } else { state = 0 ; } IntervalSet(dt); } void IntervalSet(float interupt_time) { attachTimerInterrupt(timer_1, uint64_t(interupt_time * 1000000)); } void loop() { W_L(); } uint64_t timer_1() {// timer_flg = true; return uint64_t(dt * 1000000); } void wisun_send() { unsigned char msg_length = 0 ; boolean rc = 0 ; // delay(500); switch (state) { case (0): // need hardware reset rc = bp35c0j11.cmd_send(CMD_RESET); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 1 ; } break; case (1): // init state rc = bp35c0j11.cmd_send(CMD_INI); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 2; } break; case (2): // HAN PANA setting rc = bp35c0j11.cmd_send(CMD_PANA_SET); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 4; } break; case (3): // active scan rc = bp35c0j11.cmd_send(CMD_SCAN); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 4; } } break; case (4): // HAN act rc = bp35c0j11.cmd_send(CMD_HAN); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 5; } break; case (5): // HAN PANA act rc = bp35c0j11.cmd_send(CMD_PANA); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 7; } } break; case (6): // rcv mode change rc = bp35c0j11.cmd_send(CMD_CON_SET); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 7; } break; case (7): // my_port open rc = bp35c0j11.cmd_send(CMD_PORTOPEN); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { state = 8; } break; case (8): // UDP send char charparam1[9]; //mpu_t word ushTmp; unsigned char dtmp[2]; ushTmp = (word)(mpu_t / 600.0 * 65535.0); bp35c0j11.radiodata[0] = (unsigned char)(ushTmp >> 8 & 0x00ff); bp35c0j11.radiodata[1] = (unsigned char)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[4] + 1.0) / 2.0 * 65535.0); bp35c0j11.radiodata[2] = (byte)(ushTmp >> 8 & 0x00ff); bp35c0j11.radiodata[3] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[5] + 1.0) / 2.0 * 65535.0); bp35c0j11.radiodata[4] = (byte)(ushTmp >> 8 & 0x00ff); bp35c0j11.radiodata[5] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[6] + 1.0) / 2.0 * 65535.0); bp35c0j11.radiodata[6] = (byte)(ushTmp >> 8 & 0x00ff); bp35c0j11.radiodata[7] = (byte)(ushTmp & 0x00ff); ushTmp = (word)((reg_data[7] + 1.0) / 2.0 * 65535.0); bp35c0j11.radiodata[8] = (byte)(ushTmp >> 8 & 0x00ff); bp35c0j11.radiodata[9] = (byte)(ushTmp & 0x00ff); rc = bp35c0j11.cmd_send(CMD_UDPSEND); rc = bp35c0j11.wait_msg(); if (rc == TRUE) { // delay(500); } break; default: // error state = 0 ; break; } } void W_L() { //データ受信し送信する関数 RPacket *rpacket; int subid; int8_t msgid; subid = 2; /* Receive message from SubCore */ int ret = MP.Recv(&msgid, &rpacket, subid); if (ret > 0) { statusmpu = rpacket->statusmpu; mpu_t = rpacket->mpu_t; reg_data[0] = rpacket->reg_data[0]; reg_data[1] = rpacket->reg_data[1]; reg_data[2] = rpacket->reg_data[2]; reg_data[3] = rpacket->reg_data[3]; reg_data[4] = rpacket->reg_data[4]; reg_data[5] = rpacket->reg_data[5]; reg_data[6] = rpacket->reg_data[6]; reg_data[7] = rpacket->reg_data[7]; reg_data[8] = rpacket->reg_data[8]; reg_data[9] = rpacket->reg_data[9]; reg_data[10] = rpacket->reg_data[10]; reg_data[11] = rpacket->reg_data[11]; if (timer_flg) { wisun_send(); timer_flg = false; } /* status -> ready */ rpacket->status = 0; } } char * dtostrf(double value, unsigned int width, unsigned int decimalPlaces, char* buf) { char fmt[20]; snprintf(fmt, 20, "%%%d.%df", width, decimalPlaces); sprintf(buf, fmt, value); return buf; } ``` ### 送信処理（Wi-SUN）ライブラリの中身 Rohmが公開している[ライブラリ](https://github.com/RohmSemiconductor/Arduino/tree/master/SPRESENSE-WISUN-EVK-701)のお世話になった。また、下記サイトを参考に各種パラメータを設定した。 [Wi-SUN Add-onボード SPRESENSE-WiSUN-EVK-701 を使ってみる](https://elchika.com/article/bb1bf833-6bd7-4c95-923f-93cf5239ef05/) ```arduino:bp35c0-j11.cpp /******************************************************************************** bp35c0-j11.cpp Copyright (c) 2019 ROHM Co.,Ltd. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. *********************************************************************************/ #include <Arduino.h> #include "bp35c0-j11.h" unsigned const char uni_req[4] = {0xD0 , 0xEA , 0x83 , 0xFC}; unsigned const char uni_res[4] = {0xD0 , 0xF9 , 0xEE , 0x5D}; unsigned const char ini_data[4] = {0x03 , 0x00 , 0x05 , 0x00}; // エンドデバイス/Sleep 非対応/922.9MHz/20mW出力 unsigned const char pair_id[8] = {0x00 , 0x1D , 0x12 , 0x91 , 0x00 , 0x02 , 0x02 , 0x3A}; // 接続先MACアドレス unsigned const char mac_adr[16] = {0xFE , 0x80 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x02 , 0x1D , 0x12 , 0x91 , 0x00 , 0x02 , 0x02 , 0x3A}; // 接続先IPv6アドレス unsigned const char my_port[2] = { 0x01 , 0x23 }; // オープンするUDPポート unsigned const char dist_port[2] = { 0x0E , 0x1A }; // 送信先UDPポート unsigned const char password[16] = { '1' , '1' , '1' , '1' , '2', '2' , '2' , '2' , '3' , '3' , '3' , '3' , '4' , '4' , '4' , '4' }; // PANA認証時のパスワード CMD_FORMAT cmd_format; BP35C0J11::BP35C0J11(void) { } /******************************************************************************** Name : j11_init Function : initial setting bp35c0-j11 input : - return : - *********************************************************************************/ void BP35C0J11::j11_init(void) { // configure output D20/D21 pinMode(PIN_ENABLE, OUTPUT); pinMode(PIN_RESET, OUTPUT); digitalWrite(PIN_ENABLE, HIGH); delay(1000); // Serial port initial Serial2.begin(115200); digitalWrite(PIN_RESET, LOW); // reset delay(500); digitalWrite(PIN_RESET, HIGH); } /******************************************************************************** Name : wait_msg Function : wait for response from bp35c0-j11 input : - return : TRUE/FALSE *********************************************************************************/ boolean BP35C0J11::wait_msg(void) { unsigned long start_time; unsigned long current_time; unsigned char rcvdata[128] = {0} ; unsigned char cnt = 0 ; start_time = millis(); while (Serial2.available() == 0) { current_time = millis(); if ((current_time - start_time) > TIMEOUT) { return FALSE; } } while (Serial2.available() > 0 ) { delay(5); rcvdata[cnt] = Serial2.read(); cnt++; if (cnt >= 128) { return FALSE; } } if (rcvdata[0] == uni_res[0] && rcvdata[1] == uni_res[1] && rcvdata[2] == uni_res[2] && rcvdata[3] == uni_res[3]) { // RESPONSE/NORTIFICATION switch (rcvdata[4] << 8 | rcvdata[5]) { case (NORT_WAKE): break; case (RES_INI): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (RES_PANA_SET): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (RES_SCAN): break; case (NORT_SCAN): break; case (RES_HAN): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (RES_PANA): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (NORT_PANA): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (RES_CON_SET): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (RES_PORTOPEN): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (RES_UDPSEND): if (rcvdata[12] == 0x01) { } else { return FALSE; } break; case (0x2FFF): return FALSE; break; default: return FALSE; break; } } else { return FALSE; } return TRUE; } /******************************************************************************** Name : cmd_send Function : REQUEST command to bp35c0-j11 input : cmd - REQUEST command return : TRUE/FALSE *********************************************************************************/ boolean BP35C0J11::cmd_send(unsigned short cmd) { unsigned short hdr_chksum = uni_req[0] + uni_req[1] + uni_req[2] + uni_req[3] ; unsigned short dat_chksum = 0 ; unsigned short msg_length = 0 ; unsigned short dat_length = 0 ; unsigned short send_dat_size = 0 ; unsigned char data[128] = {0}; unsigned char send_data[128] = {0} ; unsigned char cnt = 0 ; switch (cmd) { case (CMD_RESET): dat_length = 0; msg_length = (unsigned short)(4 + dat_length); hdr_chksum += CMD_RESET + msg_length; dat_chksum = 0 ; msg_create(CMD_RESET , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, (msg_length + CMD_HDR_LEN)); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; case (CMD_INI): dat_length = (unsigned short)4; msg_length = (unsigned short )( 4 + dat_length); hdr_chksum += CMD_INI + msg_length ; for (cnt = 0 ; cnt < dat_length ; cnt++ ) { data[cnt] = ini_data[cnt] ; } for (cnt = 0 ; cnt < dat_length ; cnt++) { dat_chksum += data[cnt]; } msg_create(CMD_INI , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, (msg_length + CMD_HDR_LEN)); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; case (CMD_PANA_SET): dat_length = (unsigned short)16 ; msg_length = (unsigned short)(4 + dat_length); hdr_chksum += CMD_PANA_SET + msg_length; for (cnt = 0 ; cnt < dat_length ; cnt++ ) { data[cnt] = password[cnt] ; } for (cnt = 0 ; cnt < dat_length ; cnt++) { dat_chksum += data[cnt]; } msg_create(CMD_PANA_SET , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, (msg_length + CMD_HDR_LEN)); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; case (CMD_SCAN): break; case (CMD_HAN): dat_length = (unsigned short)8 ; msg_length = (unsigned short)(4 + dat_length); hdr_chksum += CMD_HAN + msg_length; for (cnt = 0 ; cnt < dat_length ; cnt++ ) { data[cnt] = pair_id[cnt] ; } for (cnt = 0 ; cnt < dat_length ; cnt++) { dat_chksum += data[cnt]; } msg_create(CMD_HAN , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, (msg_length + CMD_HDR_LEN)); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; case (CMD_PANA): dat_length = 0; msg_length = (unsigned short)(4 + dat_length); hdr_chksum += CMD_PANA + msg_length; dat_chksum = 0 ; msg_create(CMD_PANA , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, msg_length + CMD_HDR_LEN); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; case (CMD_CON_SET): dat_length = 1; msg_length = (unsigned short)(4 + dat_length); hdr_chksum += CMD_CON_SET + msg_length; data[0] = 0x02 ; dat_chksum = data[0] ; msg_create(CMD_CON_SET , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, msg_length + CMD_HDR_LEN); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; case (CMD_PORTOPEN): dat_length = 2; msg_length = (unsigned short)(4 + dat_length); hdr_chksum += CMD_PORTOPEN + msg_length; for (cnt = 0 ; cnt < dat_length ; cnt++ ) { data[cnt] = my_port[cnt] ; } for (cnt = 0 ; cnt < dat_length ; cnt++) { dat_chksum += data[cnt]; } msg_create(CMD_PORTOPEN , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, msg_length + CMD_HDR_LEN); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; case (CMD_UDPSEND): send_dat_size = sizeof(radiodata); dat_length = 22 + send_dat_size ; msg_length = (unsigned short)(4 + dat_length); hdr_chksum += CMD_UDPSEND + msg_length; for (cnt = 0 ; cnt < 16 ; cnt++ ) { data[cnt] = mac_adr[cnt] ; } data[16] = my_port[0] ; data[17] = my_port[1] ; // 送信元UDPポート　：0x0123 data[18] = dist_port[0] ; data[19] = dist_port[1] ; // 送信先UDPポート：0x0E1A data[20] = (unsigned char)(send_dat_size >> 8); data[21] = (unsigned char)(send_dat_size & 0xFF); // send data length for (cnt = 0 ; cnt < send_dat_size ; cnt++) { data[22 + cnt] = radiodata[cnt]; // data } for (cnt = 0 ; cnt < dat_length ; cnt++) { dat_chksum += data[cnt]; } msg_create(CMD_UDPSEND , msg_length , hdr_chksum , dat_chksum, data , send_data ); Serial2.write(send_data, msg_length + CMD_HDR_LEN); #ifdef DEBUG debugmsg( msg_length + CMD_HDR_LEN , send_data); #endif break; default: break; } return TRUE; } /******************************************************************************** Name : msg_create Function : create Request command format input : cmd - Request command msg_length - message data length hdr_chksum - header checksum dat_chksum - data checksum pdada - wireless data psend_data- request command format data return : - *********************************************************************************/ void static BP35C0J11::msg_create(unsigned short cmd , unsigned short msg_length , unsigned short hdr_chksum , unsigned short dat_chksum, unsigned char *pdata , unsigned char *psend_data ) { unsigned char cnt = 0 ; for (cnt = 0 ; cnt < 4 ; cnt++) { psend_data[cnt] = uni_req[cnt]; } psend_data[4] = (unsigned char)((cmd & 0xFF00) >> 8); psend_data[5] = (unsigned char)(cmd & 0xFF); psend_data[6] = (unsigned char)((msg_length & 0xFF00) >> 8); psend_data[7] = (unsigned char)(msg_length & 0xFF); psend_data[8] = (unsigned char)((hdr_chksum & 0xFF00) >> 8); psend_data[9] = (unsigned char)(hdr_chksum & 0xFF); psend_data[10] = (unsigned char)((dat_chksum & 0xFF00) >> 8); psend_data[11] = (unsigned char)(dat_chksum & 0xFF); if (msg_length > 4) { for (cnt = 0 ; cnt < msg_length - 4 ; cnt++) { psend_data[12 + cnt] = pdata[cnt]; } } } /******************************************************************************** Name : debugmsg Function : output serial console for debug input : datalength - output data lengh psend_data - output data pointer return : - *********************************************************************************/ void BP35C0J11::debugmsg(unsigned short datalength , unsigned char* psend_data) { unsigned char cnt = 0 ; for ( cnt = 0 ; cnt < datalength ; cnt++) { } } ``` ```arduino:bp35c0-j11.h /* bp35c0-j11.h Copyright (c) 2019 ROHM Co.,Ltd. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef _BP35C0J11_H_ #define _BP35C0J11_H_ //#define DEBUG #define CMD_HDR_LEN ((unsigned char)8) #define UNI_CODE_LEN ((unsigned char)4) #define CMD_RESET (0x00D9) #define CMD_INI (0x005F) #define CMD_HAN (0x000A) #define CMD_PANA (0x003A) #define CMD_PANA_SET (0x002C) #define CMD_CON_SET (0x0025) #define CMD_UDPSEND (0x0008) #define CMD_SCAN (0x0051) #define CMD_PORTOPEN (0x0005) #define NORT_WAKE (0x6019) #define RES_INI (0x205F) #define RES_HAN (0x200A) #define RES_PANA (0x203A) #define RES_PANA_SET (0x202C) #define RES_CON_SET (0x2025) #define RES_UDPSEND (0x2008) #define RES_SCAN (0x2051) #define NORT_SCAN (0x4051) #define RES_PORTOPEN (0x2005) #define NORT_PANA (0x6028) #define TIMEOUT ((unsigned short)10000) #define PIN_ENABLE (PIN_D20) // level shifter enable pin #define PIN_RESET (PIN_D21) // wisun module reset pin #define TRUE 1 #define FALSE 0 typedef struct { unsigned char uni_code[4]; unsigned char cmd_code[2]; unsigned char msg_len[2]; unsigned char hdr_chksum[2]; unsigned char dat_chksum[2]; unsigned char data[128]; } CMD_FORMAT; class BP35C0J11 { public: unsigned char radiodata[10] = {0x00 , 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; BP35C0J11(void); void j11_init(void); boolean wait_msg(void); boolean cmd_send(unsigned short cmd); void static msg_create(unsigned short cmd , unsigned short msg_length , unsigned short hdr_chksum , unsigned short dat_chksum, unsigned char *pdata , unsigned char *psend_data ); private: void static debugmsg(unsigned short datalength , unsigned char *psend_data); }; #endif //_BP35C0J11_H_ ``` ## その他補助アプリケーション ### 操縦用アプリケーション ```python:main.py from pyqtgraph.Qt import QtGui, QtCore import numpy as np import pyqtgraph as pg from PyQt5 import uic import sys import glob import serial import pygame from pygame.locals import * import sys class JoyStatus: def __init__(self): self.axLx = 0 self.axLy = 0 self.axRx = 0 self.axRy = 0 self.hatL = 0 self.hatR = 0 self.hatU = 0 self.hatD = 0 self.btn = [0,0,0,0,0,0,0,0,0,0,0,0] class Position: def __init__(self): self.clear() def clear(self): self.lx = 0.0 self.ly = 0.0 self.lz = 0.0 self.rx = 0.0 self.ry = 0.0 self.rz = 0.0 def get_comport(): if sys.platform.startswith('win'): ports = ['COM%s' % (i + 1) for i in range(256)] else: raise EnvironmentError('Unsupported platform') result = [] for port in ports: try: s = serial.Serial(port) s.close() result.append(port) except (OSError, serial.SerialException): pass return result def click_start(): global st_c if st_radio == 0:#normal st_c = 3 elif st_radio == 1:#manual st_c = 4 else:#gain st_c = 5 param[0] = win.horizontalSlider_p0.value() param[1] = win.horizontalSlider_p1.value() param[2] = win.horizontalSlider_p2.value() param[3] = win.horizontalSlider_p3.value() param[4] = win.horizontalSlider_p4.value() param[5] = win.horizontalSlider_p5.value() param[6] = win.horizontalSlider_p6.value() param[7] = win.horizontalSlider_p7.value() param[8] = win.horizontalSlider_p8.value() param[9] = win.horizontalSlider_p9.value() param[10] = win.horizontalSlider_p10.value() param[11] = win.horizontalSlider_p11.value() win.pushButton_start.setEnabled(False) win.pushButton_stop.setEnabled(True) print("start") def click_stop(): global st_c st_c = 2 win.pushButton_start.setEnabled(True) win.pushButton_stop.setEnabled(False) def click_disconnect(): global con global st_c st_c = 1 con.close() win.pushButton_connect.setEnabled(True) win.pushButton_disconnect.setEnabled(False) win.pushButton_start.setEnabled(False) win.pushButton_stop.setEnabled(False) def click_connect(): global con global st_c st_c = 2 sel_com = win.comboBox_comport.currentText() sel_bau = win.comboBox_baudrate.currentText() con=serial.Serial(sel_com,int(sel_bau), timeout=0.05) win.pushButton_connect.setEnabled(False) win.pushButton_disconnect.setEnabled(True) win.pushButton_start.setEnabled(True) win.pushButton_stop.setEnabled(False) # print(result) def update(): global ctime, dt,gui_timer_cnt,send_timer_cnt,f_gui_timer,f_send_timer ctime = ctime + dt if ctime > 600.0: ctime = 0.0 gui_timer_cnt = gui_timer_cnt + 1 send_timer_cnt = send_timer_cnt + 1 if gui_timer_cnt > gui_timer_dt/dt - 1 : gui_timer_cnt = 0 f_gui_timer = 1 if send_timer_cnt > send_timer_dt/dt - 1 : send_timer_cnt = 0 f_send_timer = 1 if st_c == 2 or st_c == 3 or st_c == 4 or st_c == 5: serial_read() gui_cycle() send_cycle() # curve_dp.setData(dp_data) def gui_cycle(): global f_gui_timer global param,joy global curve_up, up_data, dp_data, up , dp ,curve_dp,st_c if f_gui_timer == 1: win.label_status.setText("Robot Status:"+str(st_r)) roll = [r[0] for r in up_data] pitch = [r[1] for r in up_data] yaw = [r[2] for r in up_data] curve_up[0].setData(roll) curve_up[1].setData(pitch) curve_up[2].setData(yaw) com4 = [r2[0] for r2 in dp_data] com5 = [r2[1] for r2 in dp_data] com6 = [r2[2] for r2 in dp_data] com7 = [r2[3] for r2 in dp_data] curve_dp[0].setData(com4) curve_dp[1].setData(com5) curve_dp[2].setData(com6) curve_dp[3].setData(com7) if st_joy == 1: # joypad_update() #roll # param[8] = int((setMax-setMin)*(-joy.get_axis(3) + 1.0)/2.0) win.horizontalSlider_p8.setValue(int(param[8])) win.label_p8.setText('p8:{:>3}'.format(int(param[8]))) #pitch # param[9] = int((setMax-setMin)*(-joy.get_axis(2) + 1.0)/2.0) win.horizontalSlider_p9.setValue(int(param[9])) win.label_p9.setText('p9:{:>3}'.format(int(param[9]))) #yaw # param[10] = int((setMax-setMin)*(joy.get_axis(0) + 1.0)/2.0) win.horizontalSlider_p10.setValue(int(param[10])) win.label_p10.setText('p10:{:>3}'.format(int(param[10]))) #th # param[11] = int((setMax-setMin)*(-joy.get_axis(1) + 1.0)/2.0) win.horizontalSlider_p11.setValue(int(param[11])) win.label_p11.setText('p11:{:>3}'.format(int(param[11]))) # print(joy.get_axis(1)) f_gui_timer = 0 def send_cycle(): global f_send_timer if f_send_timer == 1: if st_c == 3 or st_c == 4 or st_c == 5: serial_send() f_send_timer = 0 def serial_send(): global con # print("send") #get send data send_param = [] send_data = bytearray() #start byte send_data.append(0xa5) send_data.append(0x5b) #status # print(bytes([st_c])) send_data.append(st_c) #time tmp = int(ctime/600.0 * 65535.0) tmp_byte = tmp.to_bytes(2, 'big') send_data.append(tmp_byte[0]) send_data.append(tmp_byte[1]) joypad_update() param[8] = int((setMax-setMin)*(-joy.get_axis(3) + 1.0)/2.0) param[9] = int((setMax-setMin)*(-joy.get_axis(2) + 1.0)/2.0) param[10] = int((setMax-setMin)*(joy.get_axis(0) + 1.0)/2.0) param[11] = int((setMax-setMin)*(-joy.get_axis(1) + 1.0)/2.0) for i in range(len(param)): send_param.append(int((param[i]+setMin)/(setMax-setMin)*65535.0)) for i in range(len(param)): #param tmp_byte = send_param[i].to_bytes(2, 'big') send_data.append(tmp_byte[0]) send_data.append(tmp_byte[1]) sum = 0 for item in send_data: sum=sum+item checksum = sum % 256 #stop byte send_data.append(0x0d) send_data.append(0x0a) # print(len(send_data)) con.write(send_data) def serial_read(): global con global data_buf,st_r,rtime global up_data ,dp_data data = con.readline() if len(data) > 0: for i in range(len(data)): data_buf.append(data[i]) # while len(data_buf) > 255: # data_buf.pop(0) for i in range(len(data_buf)): if len(data_buf) < 31: break addr = 0 if data_buf[i + addr] == 0xa5: addr = addr + 1 if data_buf[i + addr] == 0x5a: addr = addr + 1 st_r = int(data_buf[i + addr]) addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') rtime = float(ushTmp) / 65535.0 * 600.0 #オイラー角 roll addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') roll = float(ushTmp) / 65535.0 * 2.0 * np.pi - np.pi roll = roll*180/np.pi #オイラー角 pitch addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') pitch = float(ushTmp) / 65535.0 * 2.0 * np.pi - np.pi pitch = pitch*180/np.pi #オイラー角 yaw addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') yaw = float(ushTmp) / 65535.0 * 2.0 * np.pi - np.pi yaw = yaw*180/np.pi #高度 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') hight = float(ushTmp) / 65535.0 * 2000.0 #コマンド0 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com0 = float(ushTmp) / 65535.0 * 2.0 - 1.0 #コマンド1 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com1 = float(ushTmp) / 65535.0 * 2.0 - 1.0 #コマンド2 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com2 = float(ushTmp) / 65535.0 * 2.0 - 1.0 #コマンド3 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com3 = float(ushTmp) / 65535.0 * 2.0 - 1.0 #コマンド4 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com4 = float(ushTmp) / 65535.0 * 2.0 - 1.0 #コマンド5 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com5 = float(ushTmp) / 65535.0 * 2.0 - 1.0 #コマンド6 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com6 = float(ushTmp) / 65535.0 * 2.0 - 1.0 #コマンド7 addr = addr + 1 low_byte = data_buf[i + addr] addr = addr + 1 high_byte = data_buf[i + addr] ushTmp = int.from_bytes((low_byte ,high_byte), byteorder='big') com7 = float(ushTmp) / 65535.0 * 2.0 - 1.0 addr = addr + 1 if data_buf[i + addr] == 0x0d: addr = addr + 1 if data_buf[i + addr] == 0x0a: #data取得成功 for j in range(i + addr + 1): data_buf.pop(0) up_data.append((roll,pitch,yaw)) dp_data.append((com4,com5,com6,com7)) if len(up_data) > 100: up_data.pop(0) if len(dp_data) > 100: dp_data.pop(0) win.label_rtime.setText('Robot Time::{:.2f}'.format(float(rtime))) # print(up_data) break def closeEvent(self): global con , st_c st_c = 0 try: con.close() print("COM CLOSE") print("EXIT") except NameError: print("EXIT") def Slider_p0_value_change(): global param param[0] = win.horizontalSlider_p0.value() win.label_p0.setText('p0:{:>3}'.format(int(param[0]))) def Slider_p1_value_change(): global param param[1] = win.horizontalSlider_p1.value() win.label_p1.setText('p1:{:>3}'.format(int(param[1]))) def Slider_p2_value_change(): global param param[2] = win.horizontalSlider_p2.value() win.label_p2.setText('p2:{:>3}'.format(int(param[2]))) def Slider_p3_value_change(): global param param[3] = win.horizontalSlider_p3.value() win.label_p3.setText('p3:{:>3}'.format(int(param[3]))) def Slider_p4_value_change(): global param param[4] = win.horizontalSlider_p4.value() win.label_p4.setText('p4:{:>3}'.format(int(param[4]))) def Slider_p5_value_change(): global param param[5] = win.horizontalSlider_p5.value() win.label_p5.setText('p5:{:>3}'.format(int(param[5]))) def Slider_p6_value_change(): global param param[6] = win.horizontalSlider_p6.value() win.label_p6.setText('p6:{:>3}'.format(int(param[6]))) def Slider_p7_value_change(): global param param[7] = win.horizontalSlider_p7.value() win.label_p7.setText('p7:{:>3}'.format(int(param[7]))) def Slider_p8_value_change(): global param param[8] = win.horizontalSlider_p8.value() win.label_p8.setText('p8:{:>3}'.format(int(param[8]))) def Slider_p9_value_change(): global param param[9] = win.horizontalSlider_p9.value() win.label_p9.setText('p9:{:>3}'.format(int(param[9]))) def Slider_p10_value_change(): global param param[10] = win.horizontalSlider_p10.value() win.label_p10.setText('p10:{:>3}'.format(int(param[10]))) def Slider_p11_value_change(): global param param[11] = win.horizontalSlider_p11.value() win.label_p11.setText('p11:{:>3}'.format(int(param[11]))) def check_radio(): global st_radio if win.radioButton_normal.isChecked(): st_radio = 0 elif win.radioButton_manual.isChecked(): st_radio = 1 else: st_radio = 2 def joypad_update(): for event in pygame.event.get(): if event.type == QUIT: sys.exit() # Stick JStat.axLx = joy.get_axis(0) JStat.axLy = joy.get_axis(1) JStat.axRx = joy.get_axis(2) JStat.axRy = joy.get_axis(3) # HatSwitch (cross-key) hat_input = joy.get_hat(0) if hat_input[0] < 0: JStat.hatL = 0; JStat.hatR = 1 elif hat_input[0] > 0: JStat.hatL = 1; JStat.hatR = 0 else: JStat.hatL = 1; JStat.hatR = 1 if hat_input[1] < 0: JStat.hatD = 0; JStat.hatU = 1 elif hat_input[1] > 0: JStat.hatD = 1; JStat.hatU = 0 else: JStat.hatD = 1; JStat.hatU = 1 # Button for i in range(12): if joy.get_button(i) == 1: JStat.btn[i] = 0 else: JStat.btn[i] = 1 JStat = JoyStatus() Pos = Position() st_joy = 1 #Initialize JOYSTICK pygame.joystick.init() try: joy = pygame.joystick.Joystick(0) print("Number of Button : " + str(joy.get_numbuttons())) print("Number of Axis : " + str(joy.get_numaxes())) print("Number of Hats : " + str(joy.get_numhats())) except pygame.error: print("Joystick is not found") st_joy = 0 pygame.init() ctime = 0 dt = 0.01 gui_timer_dt = 0.02 send_timer_dt = 0.02 f_gui_timer = 0 f_send_timer = 0 gui_timer_cnt = 0 send_timer_cnt = 0 setMax = 1000 setMin = 0 st_c = 0 st_r = 0 rtime = 0 st_radio = 0 data_buf = [] up_data = [] dp_data = [] param = 50*np.ones(12) app = QtGui.QApplication([]) win = uic.loadUi("drone_controller/v004/main.ui") win.closeEvent = closeEvent # win = pg.GraphicsWindow(title="Basic plotting examples") win.resize(1000,600) win.setWindowTitle('robot controller') pg.setConfigOptions(antialias=True) win.pushButton_connect.clicked.connect(click_connect) win.pushButton_disconnect.clicked.connect(click_disconnect) win.pushButton_start.clicked.connect(click_start) win.pushButton_stop.clicked.connect(click_stop) win.pushButton_connect.setEnabled(True) win.pushButton_disconnect.setEnabled(False) win.pushButton_start.setEnabled(False) win.pushButton_stop.setEnabled(False) up = win.graphicsView.addPlot(title="robot attitude") dp = win.graphicsView_2.addPlot(title="plot") curve_up = [] curve_up.append(up.plot(pen='r', name="roll")) curve_up.append(up.plot(pen='g', name="pitch")) curve_up.append(up.plot(pen='b', name="yaw")) curve_dp = [] curve_dp.append(dp.plot(pen='r', name="com0")) curve_dp.append(dp.plot(pen='g', name="com1")) curve_dp.append(dp.plot(pen='b', name="com2")) curve_dp.append(dp.plot(pen='y', name="com3")) comport_list = get_comport() for i in range(len(comport_list)): win.comboBox_comport.addItem(str(comport_list[i])) win.comboBox_baudrate.addItem("9600") win.comboBox_baudrate.addItem("19200") win.comboBox_baudrate.addItem("38400") win.comboBox_baudrate.addItem("57600") win.comboBox_baudrate.addItem("115200") # ラジオボタンオブジェクトのグループ登録 win.radioButton_manual.toggled.connect(check_radio) win.radioButton_normal.toggled.connect(check_radio) win.radioButton_gain.toggled.connect(check_radio) win.radioButton_manual.setChecked(True) win.horizontalSlider_p0.setMaximum(setMax) win.horizontalSlider_p0.setMinimum(setMin) win.horizontalSlider_p0.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p0.valueChanged.connect(Slider_p0_value_change) win.label_p0.setText('p0:{:>3}'.format(int(win.horizontalSlider_p0.value()))) win.horizontalSlider_p1.setMaximum(setMax) win.horizontalSlider_p1.setMinimum(setMin) win.horizontalSlider_p1.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p1.valueChanged.connect(Slider_p1_value_change) win.label_p1.setText('p1:{:>3}'.format(int(win.horizontalSlider_p1.value()))) win.horizontalSlider_p2.setMaximum(setMax) win.horizontalSlider_p2.setMinimum(setMin) win.horizontalSlider_p2.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p2.valueChanged.connect(Slider_p2_value_change) win.label_p2.setText('p2:{:>3}'.format(int(win.horizontalSlider_p2.value()))) win.horizontalSlider_p3.setMaximum(setMax) win.horizontalSlider_p3.setMinimum(setMin) win.horizontalSlider_p3.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p3.valueChanged.connect(Slider_p3_value_change) win.label_p3.setText('p3:{:>3}'.format(int(win.horizontalSlider_p3.value()))) win.horizontalSlider_p4.setMaximum(setMax) win.horizontalSlider_p4.setMinimum(setMin) win.horizontalSlider_p4.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p4.valueChanged.connect(Slider_p4_value_change) win.label_p4.setText('p4:{:>3}'.format(int(win.horizontalSlider_p4.value()))) win.horizontalSlider_p5.setMaximum(setMax) win.horizontalSlider_p5.setMinimum(setMin) win.horizontalSlider_p5.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p5.valueChanged.connect(Slider_p5_value_change) win.label_p5.setText('p5:{:>3}'.format(int(win.horizontalSlider_p5.value()))) win.horizontalSlider_p6.setMaximum(setMax) win.horizontalSlider_p6.setMinimum(setMin) win.horizontalSlider_p6.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p6.valueChanged.connect(Slider_p6_value_change) win.label_p6.setText('p6:{:>3}'.format(int(win.horizontalSlider_p6.value()))) win.horizontalSlider_p7.setMaximum(setMax) win.horizontalSlider_p7.setMinimum(setMin) win.horizontalSlider_p7.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p7.valueChanged.connect(Slider_p7_value_change) win.label_p7.setText('p7:{:>3}'.format(int(win.horizontalSlider_p7.value()))) win.horizontalSlider_p8.setMaximum(setMax) win.horizontalSlider_p8.setMinimum(setMin) win.horizontalSlider_p8.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p8.valueChanged.connect(Slider_p8_value_change) win.label_p8.setText('p8:{:>3}'.format(int(win.horizontalSlider_p8.value()))) win.horizontalSlider_p9.setMaximum(setMax) win.horizontalSlider_p9.setMinimum(setMin) win.horizontalSlider_p9.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p9.valueChanged.connect(Slider_p9_value_change) win.label_p9.setText('p9:{:>3}'.format(int(win.horizontalSlider_p9.value()))) win.horizontalSlider_p10.setMaximum(setMax) win.horizontalSlider_p10.setMinimum(setMin) win.horizontalSlider_p10.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p10.valueChanged.connect(Slider_p10_value_change) win.label_p10.setText('p10:{:>3}'.format(int(win.horizontalSlider_p10.value()))) win.horizontalSlider_p11.setMaximum(setMax) win.horizontalSlider_p11.setMinimum(setMin) win.horizontalSlider_p11.setValue(int((setMax-setMin)/2)) win.horizontalSlider_p11.valueChanged.connect(Slider_p11_value_change) win.label_p11.setText('p11:{:>3}'.format(int(win.horizontalSlider_p11.value()))) timer = QtCore.QTimer() timer.timeout.connect(update) timer.start(int(dt*1000)) win.show() if __name__ == '__main__': if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'): QtGui.QApplication.instance().exec_() ``` ### wi-sun用アプリケーション 　wi-sunから送られてきたデータを可視化するアプリケーション 　COMポートは各自読み替える必要あり。 ```python:main.py import serial import datetime import numpy as np import time import pyqtgraph as pg from pyqtgraph.Qt import QtCore, QtGui import sys import matplotlib.pyplot as plt from drawnow import drawnow, figure from time import sleep con=serial.Serial('COM7',115200) print(con.portstr) win = pg.GraphicsWindow() win.setWindowTitle('direction plot') plt = win.addPlot() plt.setYRange(-0.5, 0.5) curve = plt.plot(pen=(255, 0, 0)) datax = np.zeros(50) datay = np.zeros(50) init_flg = 0 init_time = 0 def update(): global init_flg,init_time,datax,datay str_bf=con.readline() if str_bf !="" and len(str_bf) == 138: get_data = np.zeros(10,dtype=int) get_data[0] = str_bf[126] get_data[1] = str_bf[127] get_data[2] = str_bf[128] get_data[3] = str_bf[129] get_data[4] = str_bf[130] get_data[5] = str_bf[131] get_data[6] = str_bf[132] get_data[7] = str_bf[133] get_data[8] = str_bf[134] get_data[9] = str_bf[135] mpu_time = int.from_bytes((get_data[0] ,get_data[1]), byteorder='big')/65535*600 side_angle = int.from_bytes((get_data[2] ,get_data[3]), byteorder='big')/65535*2 - 1 rect_size = int.from_bytes((get_data[4] ,get_data[5]), byteorder='big')/65535**2 - 1 posx = int.from_bytes((get_data[6] ,get_data[7]), byteorder='big')/65535*2 - 1 posy = int.from_bytes((get_data[8] ,get_data[9]), byteorder='big')/65535*2 - 1 print('{0:.4f}'.format(mpu_time),'{0:.4f}'.format(side_angle)) if init_flg == 0: init_time = mpu_time init_flg = 1 datay = np.delete(datay, 0) datay = np.append(datay, rect_size) curve.setData(datay) timer = QtCore.QTimer() timer.timeout.connect(update) timer.start(50) def sendcmd(cmd_str): con.write(cmd_str.encode()) while True: str_bf=con.readline() if str_bf !="": print(str_bf) if str_bf == b'OK\r\n': break sleep(0.5) if __name__ == "__main__": sendcmd('SKSREG SF0 1\r\n') sendcmd('SKSREG S2 23\r\n') sendcmd('SKSREG S3 5678\r\n') sendcmd('SKSREG SA9 1\r\n') sendcmd('SKSREG SA2 1\r\n') sendcmd('SKSTART\r\n') sendcmd('SKSETHPWD 001D129100035840 1111222233334444\r\n') sendcmd('SKINFO\r\n') if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'): QtGui.QApplication.instance().exec_() ```