はじめに

SPRESENSEでドローンを作ったので報告連絡相談。
　目標は下記の通り。
ミニマムサクセス　：ホバリング
フルサクセス　　　：高さ制御
エキストラサクセス：マーカーで位置制御
　なお、下記のモニター提供品をモニター提供していただいた。
提供品１　Spresense メインボード
　　　　・用途　主演算装置
提供品２　Spresense 拡張ボード
　　　　・用途　IO拡張
提供品３　Spresense カメラボード（通常カメラ）
　　　　・用途　マーカー検出用
提供品４　Wi-SUN モジュール
　　　　・用途　各種センサ値取得用

上記提供品の使用箇所を下記図に示す。

システム構成

以下にシステム構成を示す。

　下記にドローンの外観を示す。

　Wi-SUNモジュールがカメラケーブルと干渉することに組み立てて初めて気が付いた。ドローンの外観下部に示すようにカメラケーブルを90度折り曲げてWi-SUNモジュールを装着している。
　以下に配線図を示す。

提供品含めたパーツリストを下記に示す。

Spresense メインボード
Spresense 拡張ボード
Spresense カメラボード（通常カメラ）
Spresense Wi-SUN モジュール
ブラシ付きDCモータ(Tello互換品)
プロペラ(Tello互換品)
加速度・ジャイロセンサ(BMI160)
距離センサ(GPY2Y0E03)
Lipoバッテリ 1s
XCL1035(5V DCDC)
RB160M-30TR(ショットキーバリアダイオード)
AP7333(3.3Vレギュレータ)
AO3400A(Nch-FET)
チップ抵抗(10kΩと100Ω)
フレーム(3Dプリンタ出力)
モータードライバ用基板(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で最も大きいほうを選択するようにした。

　矩形判別には縦横比、充填率、面積を閾値に設定し、範囲外の領域は除外して最終的に残った領域が矩形であるというような方針をとっている。

ここにツイートが表示されます
位置推定
　透視投影変換によって姿勢位置を計算したいところだが、計算負荷などの問題でできなかったため簡易的な手法で代用した。
　マーカーから得られる矩形の４つの頂点から、方向、水平距離、前後距離を算出した。方向は矩形の垂直方向の２辺の比によって算出し、水平距離は矩形中心の水平方向pixel位置とし、前後距離は矩形の面積とした。
　ドローン水平方向移動と水平距離取得データの様子を下記に示す。データはWi-SUNモジュールで取得している。

ここにツイートが表示されます

　ドローンヨー方向回転と方向取得データの様子を下記に示す。

ここにツイートが表示されます
センサ値取得
　センサーは加速度、角速度（ジャイロ）を有するBMI160、距離センサのGPY2Y0E03の２種類を使用した。
BMI160の使用に関して下記のライブラリのお世話になった。
BMI160-Arduino
姿勢推定
　姿勢推定は、ドローン制御によく用いられているMadgwickフィルタを使用した。
　下記ライブラリのお世話になった。
　MadgwickAHRS
高度推定
　センサーはドローン底部に固定するため、姿勢が傾くと地面からの距離が大きく計測されそのままでは制御に使用できない。センサ取り付け位置も影響することに注意が必要である。推定した姿勢角と距離センサの計測値からドローンと地面からの高度を推定した。
制御量算出
　角速度を制御するインナーループと姿勢角を制御するアウターループの階層構造を有するPD制御とした。
　高さ制御は、単純なPD制御にドローン重量分の推力オフセットを与えた。積分値を制御に使いたくない派なのでこのような実装になっているが、単純にPID制御で実装したほうが性能はいいかもしれない。
　位置制御に関しては水平、前後、方向指示値を姿勢角にゲインをかけた指示値として与えている。単純なP制御のみ。マーカー水平距離計測値と目標位置の偏差をヨー角、マーカー前後距離計測値と目標位置の偏差をピッチ角、マーカー方向計測値と目標方位の偏差をロール角の指示値にしている。
　制御周期は50Hzとしている。
PWM出力
　算出した制御量に基づいてPWM出力を行う。Arduino IDEを使用する場合、標準ライブラリのanalogwriteは8bit分解能のduty比しか操作できないが、本ページのようにして高分解能のPWM出力を使用することができる。

その他補足事項

操縦系については下記のようなソフトをPythonで作った。GUIが簡単に作れるので便利。

ここにツイートが表示されます

　操作モードは下記の三種類

制御モード
　制御を実施する。スロットル、ロール、ピッチ、ヨーの操作はジョイパッドで行う。GUI上のスライダでゲインを調整できる。
モータ動作確認モード
　GUI上のスライダの操作によってモータが回転する。スライダ1~4がモータ1~4に対応している。
キャリブレーションモード
　ジャイロセンサのバイアス除去とMadgwickフィルタのクォータニオンリセットを実施する。

まとめ

実際に動作している様子を下記に示す。
　このようにフルサクセスの高さ制御は達成できたが、エキストラサクセスのマーカーによる制御はイマイチうまくいっていない。
　下記に高さ制御で飛行しているドローンの様子を示す。

ここにツイートが表示されます

下記にマーカーによる定位置制御を試みているドローンの様子を示す。
※スロットルON or OFFの指示のみ与えている。

ここにツイートが表示されます

ここにツイートが表示されます

ここにツイートが表示されます

ここにツイートが表示されます

　ドローンはマーカーの方向を向くが水平・前後方向にはふらふらしてしまう。ゲイン上げ過ぎるとハンチングするし小さすぎると追従しない。
　うまく定位置制御できないのは、単純にホバリングの安定性が悪くカメラ画角からマーカーが外れてしまうことや、またマーカー方向計測値の粗さなどが原因と考えられる。
　ホバリングの安定性については、ゲインチューニングを繰り返してもこれが限界のようだった。これまでに同じスケールのドローンを作成したことがあり、制御周期がホバリングの安定性に効くことを経験した。100Hzでは安定したホバリングが実現できたが50Hzではイマイチだった。今回制御周期が50Hzなのは単純に100Hzのサイクルでは処理が追い付かなかったため。処理の軽量化は頑張ればできるのかもしれないが、時間の都合上妥協してしまった。
　マーカー方位計測値については、矩形のサイズが計測値分解能に与える影響がでかいことが考えられる。カメラで取得している画像解像度が96x64で、映った矩形のサイズは一辺当たり20~10pixel程度と小さい。マーカーに対してカメラが10度程度傾いてやっと計測値が動くくらいのなので非常に粗い計測しかできていない。かといって解像度を増やすと更新周期が遅くなるのでなかなか難しい。

今後、処理の軽量化を進めて安定したホバリングの実現を目指すとともに、マーカーによる位置制御もしっかり実現させたい。
　ドローンの制御と画像処理が同時・リアルタイムで走るのは凄い。Spresenseのパワフルさを実感した。

ソースコード

ソースコードを下記に示す。

SubCore1

受信処理

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

画像取得＆前処理＆位置推定

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_h 160
#define image_v                 (64)
//#define image_v 120

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 image_copy2()
{
  int x, y, cnt;
  cnt = 0;
  for (y = 0; y < image_v; y++) {
    for (x = 0; x < image_h; x++) {
      image_data[cnt] = image[x][y];
      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 / (double)w / (double)h;
      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 {
        //矩形頂点
        image[pos0[0][0]][pos0[0][1]] = 0;
        image[pos0[1][0]][pos0[1][1]] = 0;
        image[pos0[2][0]][pos0[2][1]] = 0;
        image[pos0[3][0]][pos0[3][1]] = 0;


        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出力＆送信処理

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 Si1buf_size 256
#define Si1readbyte_len 31
#define SUBID "Sub2"

#define MOT0 9
#define MOT1 6
#define MOT2 5
#define MOT3 3

#define batPin A0

#define s_ulim 1.0
#define s_llim 0.0

#define pulse_max 254
#define pulse_min 0

#define i2c_addr 0x68
#define MY_MSGID 11

#define dist_addr 0x40

struct MPacket {
  volatile int status; /* 0:ready, 1:busy */
  int statusmain = 0;
  float main_t = 0.0;
  float paramdata[12];
};

struct RPacket {
  volatile int status; /* 0:ready, 1:busy */
  int statuspc = 0;
  float pc_t = 0.0;
  float paramdata[12];
};

struct SPacket {
  volatile int status; /* 0:ready, 1:busy */
  int statusmpu = 0;
  float mpu_t = 0.0;
  float reg_data[12];
};

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;
byte Serial1_buffer[Si1buf_size];
int Serial1_buffer_len = 0;

unsigned long cnt0 = 0;//時間カウンタ
uint16_t cnt1 = 0;//LEDカウンタ
uint16_t cnt2 = 0;//データ送信カウンタ
uint16_t cnt3 = 0;//制御カウンタ
uint16_t cnt4 = 0;//計測カウンタ
int control_state1 = 0;
int control_state2 = 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 tof_data = 0.0;
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 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 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() {
  //  Serial.println("loop");
  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;
  /* Receive message from SubCore */

  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];

    /* status -> ready */
    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];
    float side_angle = mparamdata[0] * 1000 / 150;
    if (side_angle > 1.0) {
      side_angle = 1.0;
    }
    if (side_angle < -1.0) {
      side_angle = -1.0;
    }

    /* status -> ready */
    mpacket->status = 0;
  }

}

void control_init(void) {
  //  statusmpu = 1;
}

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.000000 * paramdata[1] + 0.0;
  float Dgain = 0.0001 * paramdata[2] + 0.0001 * 2.4;

  float YPgain = 0.01 * paramdata[3] + 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;

  float side_angle = mparamdata[0] * 1000 / 150;
  if (side_angle > 1.0) {
    side_angle = 1.0;
  }
  if (side_angle < -1.0) {
    side_angle = -1.0;
  }

 float front_dist = mparamdata[1] - 0.4;
  ang_controll((paramdata[8] - front_dist ) * 20.0 / 180.0 * M_PI, 0); //pitch
  ang_controll((paramdata[9] + side_angle * 1.5 ) * 10.0 / 180.0 * M_PI, 1); //roll

  angvel_controll(ang_controllout[0], 0);//pitch
  angvel_controll(ang_controllout[1], 1);//roll
  angvel_controll(paramdata[10] * 100.0 - mparamdata[2] * 100.0, 2); //yaw


  hight_gain[0] = 2.0;
  hight_gain[1] = 0;
  hight_gain[2] = 1.0;
  hight_controll(paramdata[11]);
  float throt = hight_controllout;//hight_controllout;
  float roll_offset = -0.08 + paramdata[4];
  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 ang_controll(float ref_ang, int target) {//target:0→roll 1→pitch 2→yaw
  float err = ref_ang - euler_data[target];//ang_gain[3]
  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 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) {
  _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) {
  _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) {
  _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) {
  _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) {
  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) {
  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 offset_l = 0.0;
  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]);
  //  float hight_datac = offset_l * (sinf(euler_data[0]) + sinf(euler_data[1])) + buf_data / 100.0f * cosf(euler_data[0]) * cosf(euler_data[1]);

  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;
  }
//  printf("%f\r\n", hight_data * 1000.0);

}

SubCore3

送信処理（Wi-SUN）

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が公開しているライブラリのお世話になった。

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++) {

  }
}

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_

その他補助アプリケーション

操縦用アプリケーション

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ポートは各自読み替える必要あり。

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_()

コメント無し