はじめに

HDRカメラボードの高品質な画像を無線で飛ばすモジュール、ラジコンとかに載せたら楽しそうだしその他いろいろ役立ちそうだなと思ったので、ひとまずドローンに載せることを想定して作ることにしました。
　本ページで紹介する作品は「2024年 SPRESENSE™ 活用コンテスト by ソニーセミコンダクタソリューションズ株式会社」に応募するものであり、以下画像に示すSpresense メインボード、Spresense HDRカメラボードは、株式会社電波新聞社 Elchika事務局様からモニター提供を受けたものです。

作品概要

システム全体像を撮影した状況を以下に示します。

　動作状況は以下のとおりです。

外観は以下の写真のとおりです。

以下に伝送された映像をキャプチャーした動画を示します。すこし長い動画なので途中早送りにしています。画面左上にバッテリー残量、衛星補足数、ドローンのステータスを表示しております。画面中央付近には、ドローンの姿勢角(ロール、ピッチ角)に基づいた水平方向マーカーを表示しています。

　
　映像の解像度はQVGAで、各フレームの画像データフォーマットはJPEGです。JPEGのクオリティーファクターは40に設定しているので、表示される映像に精細さが無いという意味では低画質ですが、HDR機能の効果で明暗差の激しい環境でも全体的に明瞭に写り、データ量は5k~10kbyte程度と非常に軽量です。
　本作品の性能を市販のデジタルVTXと比較すると、フレームレート、遅延、解像度の点では劣っていますが、ダイナミックレンジの広さであったり、カメラ設定や通信データを任意に調整できる点で優れていると思います。
　本作品では無線通信のためにSpresense Wi-Fi Add-onボード iS110Bを使用しており、公式ページで以下の通り

GS2200MIZは最大65Mbpsをサポートしますが、通信速度はSPRESENSEのSPIクロックに依存します。

と説明があります。　また、公式サンプルコードによると、１回の通信で送信できるデータ量が最大1460byteであり、筆者の動作環境ではこれを継続的に安定して伝送するためには、10ミリ秒ほど送信間隔をあける必要があることが実験の結果わかりました。なお、これは動作環境によるので、筆者の動作環境で実施した結果そうであったという話に過ぎないことにご留意ください。
　前述の前提で、実際の通信速度は1460[byte]×8[bit/byte]÷0.01[秒]=1,168,000[bps]≒1.2Mbpsであり、本来の2%程度しか実力を発揮できていません。フレームレート、遅延については、本来の通信速度に近づくように工夫することでまだまだ良い結果が期待できると思います。今後、iS110Bの仕様をより詳細に調査するなどして通信速度を改善することを検討したいと思います。

全体構成

以下に、全体構成図を示します。

映像伝送モジュール

映像伝送モジュールで使用したパーツは下記表のとおりです。

表示器

伝送されたデータを受信・処理・表示するために、ラズパイ4にwifiドングルを刺したものと小型モニターを使用しました。

ドローン

ドローンはu99クラスのFPVドローンに使用されるフライトコントローラー(以降、FCと呼称)＆ESCボードをベースに各種パーツを揃えて作成しました。なお、ファームウェア(以降、FW)はINAV6.0ベースのFLYWOO社製カスタムFWを使用しました。

作成過程

作成過程をハードウェアとソフトウェアに分けて紹介します、

ハードウェア

構造

まずSpresenseメインボートとHDRカメラをドローンへ固定するためのパーツを作成しました。3Dプリンターで作成しており、3Dモデルの作成編集はDesignSpark Mechanicalを使用しました。

電装

ドローンを組み立てて、各種パーツを固定後、配線を以下の通り結線しました。ピンアサインについては、Spresense メインボートはこのページ、FCについてはこのページの資料を参考にしました。
　以下に結線図を示します。

　結線図通りに配線してハード自体は完成です。

ソフトウェア

ソフトウェアは下図の構成のとおり作成しました。ソフトウェアの機能配置はMainCoreでのみ使用できる機能（カメラとGNSS等）の制約等を受けるので、色々考えた結果こんな感じになりました。

MAVLink

MAVLinkは、ドローンなどの遠隔動作する機器の各種状態データ(緯度経度、高度、異常検知情報などなど)を通信するためのプロトコルとして使用されています。一般的には、ドローンに搭載されたテレメトリでMAVLinkプロトコルの各種データを送信し、GCS(グランドコントロールステーション)で受信するというような使われ方をするようです。
　今回はFCのUARTポートから出力されたMAVLinkプロトコルのデータを、Spresenseメインボードのソフトウェアシリアル機能を割り当てたGPIOピンで受信することにしました。通常はハードウェアシリアルを使用する方がCPU負荷的に有利ではあるのですが、本作品のように別の機器(Spresense Wi-Fi Add-onボード)が占有している場合で、通信速度がそこまで速くないとき(9600bps程度)はこういう手法を使うことも無くはないと思います。

GNSS

また、SpresenseメインボードにはGNSSの受信機能が付いており、これも活用したいということで、同じくソフトウェアシリアル機能を使ってNMEAフォーマットのデータをFCに送信しました。FCで受信したGNSSデータがMAVLinkに乗ってSpresenseメインボードに戻ってきて、それを表示器に送信するという無駄な通信経路がありますが、各種データの同期を取るという意味ではまあいいのかなとは思います。そもそも今回のGNSS更新周期は1秒と遅いので、そこまで通信の遅れは体感できないと思うので…

JPEG

HDRカメラにはJPEGエンコーダーが付いており、容易にJPEGデータを出力できるようです。JPEGにはQF(クオリティファクター)と呼ばれるパラメーターがあり、QFが小さいと強いデータ圧縮がかかり、データ容量は軽くなるものの画像の精細さ(画質)が失われます。本作品では、通信速度が性能のボトルネックになるため可能な限りデータ量を軽量にしつつも、そこそこの画質を維持したいです。そこでQFを10,20,40,60,80の5パターンで振って画像を取得し、目視で比較してみました。

主観評価ですがQF:10～20と40には画質に比較的大きな差があるように見え、また、QF:60～80まで上げてもそこまで画質は劇的に良くならない感じがしました。よって、QFはとりあえず40としました。
　QF:40のJPEGは、画像の写り方にもよりますが大体8kBになります。もう少しデータ容量を削れないか考えたところ、ヘッダデータに目をつけました。JPEGは主にヘッダデータとイメージデータに分かれており、ヘッダデータには量子化テーブル、ハフマンテーブルなど、JPEGをデコードするときに必要なデータが含まれ、イメージデータにはエンコードされた画像そのものが記録されています。以下にバイナリエディタでJPEGを表示させた状況を示します。

　ヘッダデータは、JPEGファイルの先頭マーカー「FF D8」からイメージデータの先頭マーカー「FF DA」までの間に存在し、上図の青くハイライトされた部分がヘッダデータで、それ以降がイメージデータです。ヘッダデータは同じ条件で撮影した画像であればだいたい同じになり、毎回同じデータを送信するのはもったいないので削っちゃってもいいかな～と思いました。
　このヘッダデータをあらかじめ表示器側に保存しておくことで、受信したイメージデータとヘッダデータを合体し表示させます。SpresenseHDRカメラから出力されたJPEGのヘッダデータは575byteで、このデータ量だけ軽量化できます。1パケット分(1460byte)にも満たないので、イメージデータの分割のされ方によっては効果があったりなかったりするかもしれません。無意味、自己満とか言わないで…。

UDP通信サーバー側

表示器(ラズパイ4)はUDPサーバーとして振舞うように設定しました。ラズパイ4にはもともとPCBアンテナのWiFiが付いていますが、あえてアンテナ付きのUSBのWiFiドングルを使用しています。将来的に、利得の大きいドングルに付け替えることもできますし、何よりでっかいアンテナが付いてる方がかっこいいと思うので…。
　ラズパイの環境構築については以下の通り行いました。
手順 1: 必要なパッケージのインストール
　まず、必要なソフトウェアをインストールします。

sudo apt update
sudo apt install -y hostapd dnsmasq
hostapd: WiFiアクセスポイントを作成するためのツール。
dnsmasq: DHCPサーバーとDNSリゾルバを提供するツール。

手順 2: hostapdの設定
　WiFiアクセスポイントを構成するために、設定ファイルを作成・編集します。

sudo nano /etc/hostapd/hostapd.conf
以下、hostapd.confの記載内容
interface=wlan1
driver=nl80211
ssid=”お好きなSSID名”
hw_mode=g
channel=1
wmm_enabled=0
macaddr_acl=0
auth_algs=1
ignore_broadcast_ssid=0
wpa=2
wpa_passphrase=”お好きなパスワード”
wpa_key_mgmt=WPA-PSK
rsn_pairwise=CCMP

interface=wlan1の、"wlan1"はWiFiドングルのことを示しており、wlan0はラズパイにもともとついてる方のWiFiを示しています。
次にhostapdの設定ファイルパスを指定します。

sudo nano /etc/default/hostapd
以下の行を追加します
DAEMON_CONF="/etc/hostapd/hostapd.conf"

手順 3: dnsmasqの設定
　DHCPサーバーを構成するために、dnsmasqを設定します。まずはデフォルトの設定ファイルをバックアップします。

sudo mv /etc/dnsmasq.conf /etc/dnsmasq.conf.orig
sudo nano /etc/dnsmasq.conf
以下の内容を追加します:
interface=wlan1
dhcp-range=192.168.4.2,192.168.4.20,255.255.255.0,24h

手順 4: IPアドレスの設定
　固定IPアドレスをWiFiインターフェースに設定します。

sudo nano /etc/dhcpcd.conf
以下を追加します:
interface wlan1
static ip_address=192.168.4.1/24
nohook wpa_supplicant

ネットワークインターフェースを再起動します。

sudo service dhcpcd restart

手順 5: IP転送の有効化
　Raspberry Piがデータを転送できるようにします。
　設定ファイルを編集します。

sudo nano /etc/sysctl.conf
以下の行のコメントを解除します:
net.ipv4.ip_forward=1

実行して有効化します。

sudo sysctl -p

手順 6: iptablesの設定
ネットワークパケットを転送します。

sudo iptables -t nat -A POSTROUTING -o eth0 -j MASQUERADE
sudo iptables -A FORWARD -i eth0 -o wlan0 -m state --state RELATED,ESTABLISHED -j ACCEPT
sudo iptables -A FORWARD -i wlan0 -o eth0 -j ACCEPT
sudo sh -c "iptables-save > /etc/iptables/rules.v4"

手順 7: サービスの開始と有効化
hostapdとdnsmasqを起動し、自動起動を有効にします。

sudo systemctl start hostapd
sudo systemctl enable hostapd
sudo systemctl start dnsmasq
sudo systemctl enable dnsmasq

このあとsudo rebootで再起動すると、設定したSSIDがアクセスポイント一覧に表示されるようになります。

UDP通信クライアント側

Spresense Wi-Fi Add-onボード(以降、iS110Bと呼称)はUDPクライアントとして振舞うように設定しました。
　iS110Bの通信速度を調べるため、1パケット(1460byte)を送信する時間間隔をどの程度まで短くできるかを実験したところ、「0.01秒程度」であれば継続的に安定した通信ができることがわかりました。ただし、実験環境は一般的な集合住宅内のため、ほかのWiFiも飛んでいます。そのため、「0.01秒程度」は筆者の環境での結果に過ぎず、別の場所で測定すればまた違った結果が出ると思います。
　QF:40のJPEGは大体8~10kBになるので、これを1.5kB刻み0.01秒間隔で送信すると7回の送信で一枚の画像を送れます。また、MAVLinkのデータで1パケット使うので、7+1=8回の送信で1サイクル完了とすると、最速で1/0.08=12.5[fps]の動画を表示させることができそうです。NTSCが29.97fpsなので、アナログVTXの映像と比較すると遅く感じると思います。さらに、遅延がどの程度かという問題もあります。悲しいなぁ…

性能評価

まずはフレームレートを計測します。ストップウォッチを映しこんだ映像を保存し、10秒間で何フレーム記録されたかを確認します。この様子を以下に示します。

　上記ではストップウォッチ表示が10秒から20秒にかけての動画を示しました。この動画をAviUtlに読み込ませ、フレーム数を数えたところ、118フレームでした。よって118/10=11.8[fps]となり、大体想定したフレームレートになっていることが確認できました。
　次に動画の遅延を計測します。ストップウォッチと、ストップウォッチを映した表示器の画面を、スマホで同時に撮影し、これらの表示がどの程度ずれるかを計測します。その様子を以下に示します。

この動画1フレームを抜き出した画像を以下に示します。

　ストップウォッチの表示は59.90、表示器のストップウォッチの表示は59.72なのでおよそ0.2秒の遅延があることが確認できました。

ソースコード

MainCoreソースコード

このプログラムは以下のライブラリを使用して作成されています。

1. <MP.h>``<Camera.h>``<GNSS.h>``<gpsutils/cxd56_gnss_nmea.h>:

   • GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照
   • 詳細: https://developer.sony.com/develop/spresense/
   • このライブラリの著作権: Copyright 2019 Sony Semiconductor Solutions Corporation.

2. <SoftwareSerial.h>:

   • Arduino標準ライブラリ
   • GNU Lesser General Public License (LGPL) v2.1
   • 詳細: https://www.arduino.cc/en/Reference/SoftwareSerial

各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。

MainCore
#include <MP.h>
#include <Camera.h>
#include <GNSS.h>
#include <gpsutils/cxd56_gnss_nmea.h>
#include <SoftwareSerial.h>

#define CONSOLE_BAUDRATE 1000000

//GNSS NMEAデータ送信用
SoftwareSerial mySerial_gnss(5, 14);  // RX,TXの割り当て
//NMEA buffer
static char nmea_buf[NMEA_SENTENCE_MAX_LEN];

// GNSSインスタンス
SpGnss Gnss;
GnssPositionData PositionData;

/* NMEA callback functions */
static char *reqbuf(uint16_t size) {
  /* Get the pointer to NMEA buffer */
  if (size > sizeof(nmea_buf)) {
    return NULL;
  }
  return nmea_buf;
}

static void freebuf(char *buf) {
  /* do nothing */
}

static int outbin(char *buf, uint32_t len) {
  /* unused dummy function */
  return len;
}

static int outnmea(char *buf) {
  /* Output NMEA to serial monitor */
  return mySerial_gnss.println(buf);
}


struct Core0Packet {
  volatile int status = 0;  //img_buに格納された画像が既にsubcore1が受信したものの時は0、そうでない時は1
  uint32_t img_size = 0;    //画像サイズ
  uint8_t img_buf[65536];
};
Core0Packet packet;
int subcore = 1;  //送信先subcore番号

//LED点灯状況
bool led0_flg = 0;
bool led1_flg = 0;
bool led2_flg = 0;
bool led3_flg = 0;

//カメラ画像更新
void CamCB(CamImage img) {
  if (img.isAvailable()) {                                         //カメラ画像が更新されたとき
    int8_t msgid = 10;                                             //メッセージIDなんでもええ
    if (packet.status == 0) {                                      //subcore1が前データを受信完了している場合
      memcpy(packet.img_buf, img.getImgBuff(), img.getImgSize());  //送信用構造体に画像データをコピー
      packet.img_size = img.getImgSize();                          //画像サイズ代入
      packet.status = 1;                                           //構造体に最新画像がセットされたことを示すフラグ
      subcore = 1;                                                 //送信先subcore
      MP.Send(msgid, &packet, subcore);                            //subcore1に送信用構造体を送信
    }
    led0_flg = 1 - led0_flg;       //LED1を反転
    digitalWrite(LED0, led0_flg);  //LED1点灯状況反映
  }
}

void initCamera() {
  //カメラ初期設定
  CamErr err;
  Serial.println("Prepare camera");
  err = theCamera.begin(2,
                        CAM_VIDEO_FPS_30,
                        CAM_IMGSIZE_QVGA_H,
                        CAM_IMGSIZE_QVGA_V,
                        CAM_IMAGE_PIX_FMT_JPG);
  if (err != CAM_ERR_SUCCESS) {
    Serial.println("begin ERROR");
  }

  // ホワイトバランスの設定
  Serial.println("Set Auto white balance parameter");
  err = theCamera.setAutoWhiteBalanceMode(CAM_WHITE_BALANCE_AUTO);
  if (err != CAM_ERR_SUCCESS) {
    Serial.println("setAutoWhiteBalanceMode ERROR");
  }

  // JPEG品質の設定
  Serial.println("Set JPEG quality");
  err = theCamera.setJPEGQuality(10);
  if (err != CAM_ERR_SUCCESS) {
    Serial.println("setJPEGQuality ERROR");
  }

  // HDRの設定
  Serial.println("Set HDR");
  err = theCamera.setHDR(CAM_HDR_MODE_ON);
  if (err != CAM_ERR_SUCCESS) {
    Serial.println("setHDR ERROR");
  }

  // ISOの設定
  Serial.println("Set ISO Sensitivity");
  //  err = theCamera.setISOSensitivity(CAM_ISO_SENSITIVITY_1600);
  err = theCamera.setAutoISOSensitivity(true);
  if (err != CAM_ERR_SUCCESS) {
    Serial.println("setAutoISOSensitivity ERROR");
  }

  // 露出の設定
  int exposure = 20;
  //  err = theCamera.setAutoExposure(true);
  err = theCamera.setAbsoluteExposure(exposure);
  if (err != CAM_ERR_SUCCESS) {
    Serial.println("setAbsoluteExposure ERROR");
  }

  // カメラストリームを受信したら CamCBを実行する
  Serial.println("Start streaming");
  err = theCamera.startStreaming(true, CamCB);
  if (err != CAM_ERR_SUCCESS) {
    Serial.println("startStreaming ERROR");
  }
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(CONSOLE_BAUDRATE);
  mySerial_gnss.begin(9600);  // ソフトウェアシリアルの通信速度

  /* Initialize GNSS */
  if (Gnss.begin()) {
    Serial.println("begin error!");
  }

  /* select satellite system */
  Gnss.select(GPS);
  Gnss.select(GLONASS);
  Gnss.select(QZ_L1CA);
  Gnss.select(QZ_L1S);

  /* Set interval */
  Gnss.setInterval(1);

  /* Start GNSS */
  if (Gnss.start()) {
    Serial.println("start error!");
  }

  /* Initialize NMEA library */
  NMEA_InitMask();

  /* Select NMEA sentence */
  NMEA_SetMask(NMEA_GGA_ON | NMEA_GLL_ON | NMEA_GSA_ON | NMEA_GSV_ON | NMEA_GNS_ON | NMEA_RMC_ON | NMEA_VTG_ON | NMEA_ZDA_ON);

  /* Register callbacks to output NMEA */
  NMEA_OUTPUT_CB funcs;
  funcs.bufReq = reqbuf;
  funcs.out = outnmea;
  funcs.outBin = outbin;
  funcs.bufFree = freebuf;
  NMEA_RegistOutputFunc(&funcs);

  //subcore1起動
  subcore = 1;
  int ret = 0;
  ret = MP.begin(subcore);
  if (ret < 0) {
    printf("MP.begin(%d) error = %d\n", subcore, ret);
  }

  //subcore2起動
  subcore = 2;
  ret = MP.begin(subcore);
  if (ret < 0) {
    printf("MP.begin(%d) error = %d\n", subcore, ret);
  }

  //初期設定時のコア間通信はブロッキング。後でポーリングに再設定する。
  MP.RecvTimeout(MP_RECV_BLOCKING);

  //subcore1準備完了待ち
  int8_t msgid;
  void *adder;
  subcore = 1;
  MP.Recv(&msgid, &adder, subcore);
  MP.Send(msgid, &adder, subcore);  //subcore1にmaincore準備完了送信


  //subcore1準備完了待ち
  subcore = 2;
  MP.Recv(&msgid, &adder, subcore);
  MP.Send(msgid, &adder, subcore);  //subcore2にmaincore準備完了送信

  memset(&packet, 0, sizeof(packet));  //送信用構造体初期化

  MP.RecvTimeout(MP_RECV_POLLING);  //コア間通信をポーリングに変更

  initCamera();  //カメラ初期設定
}

void loop() {
  if (Gnss.waitUpdate(-1)) {  //GNSS受信時
    SpNavData NavData;
    Gnss.getNavData(&NavData);                                                //データ取得
    led2_flg = (NavData.posDataExist && (NavData.posFixMode != FixInvalid));  //衛星補足し、測位FIXしたとき
    Gnss.getPositionData(&PositionData);                                      //位置データ抽出
    NMEA_Output(&PositionData.Data);                                          //NMEA変換
    led1_flg = 1 - led1_flg;                                                  //LED1点灯状況反転
    digitalWrite(LED1, led1_flg);                                             //LED点灯状況反映
    digitalWrite(LED2, led2_flg);                                             //衛星補足し、測位FIXしたときLED2点灯
  }
}

SubCore1ソースコード

1. <TelitWiFi.h>:

   • GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照
   • 詳細: https://github.com/jittermaster/GS2200-WiFi
   • このライブラリの著作権: Copyright 2019 Norikazu Goto.

2. <MP.h>:

   • GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照
   • 詳細: https://developer.sony.com/develop/spresense/
   • このライブラリの著作権: Copyright 2019 Sony Semiconductor Solutions Corporation.

各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。

SubCore1
#if (SUBCORE != 1)
#error "error"
#endif
#include <TelitWiFi.h>
#include <MP.h>

#define AP_SSID "MyAP"
#define PASSPHRASE "MyPassword"
#define UDPSRVR_IP "192.168.4.1"
#define UDPCLNT_IP "192.168.4.10"
#define SUBNETMASK "255.255.255.0"
#define GATEWAY_IP "192.168.4.1"
#define UDPSRVR_PORT "10001"
#define LocalPort "10001"
#define CONSOLE_BAUDRATE 1000000

//コア間通信------------------------------------------
int8_t msgid = 0;         //メッセージID
struct Core0Packet {      //画像データ用構造体
  volatile int status;    //img_buに格納された画像が既にsubcore1が受信したものの時は0、そうでない時は1
  uint32_t img_size = 0;  //画像サイズ
  uint8_t img_buf[65536];
};
Core0Packet *packet;

int subcore = 2;  //送信先コア番号

//UDP通信---------------------------------------------
TelitWiFi gs2200;
TWIFI_Params gsparams;
char server_cid = 0;                        //
const uint16_t UDP_PACKET_SIZE = 1460;      //パケット最大データ量
uint8_t UDP_Data[UDP_PACKET_SIZE] = { 0 };  //送信データ格納用

bool send_flg = 0;          //送信用画像データがmaincoreから受信されたとき1、UDPで画像送信終わった時0
uint8_t sendcycle_num = 0;  //複数パケットに分割して送信するときのパケット数（今何パケット目？）
uint32_t sended_bytes = 0;  //送信した完了した（送信しなくてもよい）データ数

//MAVLink---------------------------------------------
union mavlink_data {  //mavlink受信データ格納用構造体
  struct {
    uint8_t battery_remaining;
    uint8_t rssi;
    uint8_t satellites_visible;
    uint8_t system_status;

    uint16_t load;
    uint16_t voltage_battery;
    uint16_t current_battery;
    uint16_t chan1_raw;
    uint16_t chan2_raw;
    uint16_t chan3_raw;
    uint16_t chan4_raw;
    uint16_t chan5_raw;
    uint16_t chan6_raw;
    uint16_t chan7_raw;
    uint16_t chan8_raw;
    uint16_t vel;
    uint16_t cog;

    int16_t temperature;

    uint32_t time_boot_ms;

    int32_t lat;
    int32_t lon;
    int32_t alt;

    float press_abs;
    float roll;
    float pitch;
    float yaw;
  };
  uint8_t bin[sizeof(uint8_t) * 4 + sizeof(uint16_t) * 13 + sizeof(int16_t) * 1 + sizeof(uint32_t) * 1 + sizeof(int32_t) * 3 + sizeof(float) * 4];
};
mavlink_data *recv_data;

//タイマー---------------------------------------------
float t = 0.0;
float dt = 0.001;        //s
unsigned long cnt0 = 0;  //時間カウンタ
uint16_t cnt1 = 0;       //カウンタ１
uint16_t cnt2 = 0;       //カウンタ２
uint16_t cnt3 = 0;       //カウンタ３
uint16_t cnt4 = 0;       //カウンタ４
boolean cnt1_flag = HIGH;
boolean cnt2_flag = LOW;
boolean cnt3_flag = LOW;
boolean cnt4_flag = LOW;
float cnt1_time = 1.0;    //割り込み周期[sec]//TBD
float cnt2_time = 0.001;  //割り込み周期[sec]//UDP送信タイミングカウンタ
float cnt3_time = 0.01;   //割り込み周期[sec]//TBD
float cnt4_time = 0.01;   //割り込み周期[sec]//TBD



void IntervalSet(float interupt_time) {  //タイマー初期化
  attachTimerInterrupt(timer_1, uint64_t(interupt_time * 1000000));
}

uint64_t timer_1() {  //タイマー関数
  t = t + dt;
  cnt0 = cnt0 + 1;
  cnt1 = cnt1 + 1;
  cnt2 = cnt2 + 1;
  cnt3 = cnt3 + 1;
  cnt4 = cnt4 + 1;
  if (cnt1 > (uint16_t)(cnt1_time / dt)) {  //TBD
    cnt1_flag = !cnt1_flag;
    cnt1 = 0;
  }
  if (cnt2 > (uint16_t)(cnt2_time / dt)) {  //UDP送信タイミングカウンタ
    cnt2_flag = HIGH;
    cnt2 = 0;
  }
  if (cnt3 > (uint16_t)(cnt3_time / dt)) {  //TBD
    cnt3_flag = HIGH;
    cnt3 = 0;
  }
  if (cnt4 > (uint16_t)(cnt4_time / dt)) {  //TBD
    cnt4_flag = HIGH;
    cnt4 = 0;
  }
  return uint64_t(dt * 1000000);
}


void send_cycle() {  //UDP送信関数
  if (cnt2_flag) {
    //maincoreから画像受信
    int ret;
    ret = MP.Recv(&msgid, &packet);

    //subcore2からmavlinkデータ受信
    subcore = 2;
    int ret2;
    ret2 = MP.Recv(&msgid, &recv_data, subcore);

    //画像受信完了したとき
    if (ret > 0) {
      //画像データからイメージ情報開始アドレスを検索
      uint16_t sosmarker_adder = 0;
      for (uint16_t i = 0; i < packet->img_size - 1; i++) {  //SOS scan(0xff 0xda)
        if (packet->img_buf[i] == 0xff && packet->img_buf[i + 1] == 0xda) {
          sosmarker_adder = i;
          break;
        }
      }
      //イメージ情報開始アドレス以降をUDP送信データに格納するように設定。
      sended_bytes = sosmarker_adder;
      //画像データ受信完了フラグを立てる
      send_flg = 1;
    }

    if (send_flg) {                                            //画像情報が受信出来たとき
      if (packet->status == 1) {                               //画像データが最新のものかを確認
        uint32_t send_bytes = UDP_PACKET_SIZE - 1;             //フッタバイト分減算
        if (sendcycle_num == 0) {                              //初回パケットの時、mavlinkデータを送信する
          send_bytes = sizeof(mavlink_data);                   //mavlinkデータサイズ
          memcpy(UDP_Data, &recv_data->bin, send_bytes);       //データ格納
          UDP_Data[send_bytes] = sendcycle_num;                //データ末尾にパケット番号を付与
          gs2200.write(server_cid, UDP_Data, send_bytes + 1);  //UDP送信
          sendcycle_num = sendcycle_num + 1;                   //パケット番号カウントアップ
        } else {
          if (send_bytes > packet->img_size - sended_bytes) {
            send_bytes = packet->img_size - sended_bytes;
            if (send_bytes < 0) {
              sended_bytes = 0;    //画像データアドレスリセット
              sendcycle_num = 0;   //パケット番号リセット
              packet->status = 0;  //画像データ送信済み
              send_flg = 0;
            } else {
              memcpy(UDP_Data, &packet->img_buf[sended_bytes], send_bytes);  //データ格納
              UDP_Data[send_bytes] = sendcycle_num;                          //データ末尾にパケット番号を付与
              gs2200.write(server_cid, UDP_Data, send_bytes + 1);            //UDP送信
              sended_bytes = 0;                                              //画像データアドレスリセット
              sendcycle_num = 0;                                             //パケット番号リセット
              packet->status = 0;                                            //画像データ送信済み
              send_flg = 0;
            }

          } else {
            memcpy(UDP_Data, &packet->img_buf[sended_bytes], send_bytes);  //データ格納
            UDP_Data[send_bytes] = sendcycle_num;                          //データ末尾にパケット番号を付与
            gs2200.write(server_cid, UDP_Data, sizeof(UDP_Data));          //UDP送信
            sended_bytes = sended_bytes + send_bytes;                      //送信データ分だけ画像データアドレスカウントアップ
            sendcycle_num = sendcycle_num + 1;                             //パケット番号カウントアップ
          }
        }
      }
    }
    cnt2_flag = LOW;
  }
}

void setup() {
  Serial.begin(CONSOLE_BAUDRATE);

  MP.begin();
  MP.RecvTimeout(MP_RECV_BLOCKING);

  /* Initialize AT Command Library Buffer */
  AtCmd_Init();
  /* Initialize SPI access of GS2200 */
  Init_GS2200_SPI_type(iS110B_TypeC);
  /* Initialize AT Command Library Buffer */
  gsparams.mode = ATCMD_MODE_STATION;
  gsparams.psave = ATCMD_PSAVE_ALWAYS_ON;
  if (gs2200.begin(gsparams)) {
    ConsoleLog("GS2200 Initilization Fails");
    while (1) {}
  }

  /* GS2200 Association to AP */
  ConsoleLog("Associate to Access Point");
  ATCMD_RESP_E r;
  while (1) {
    /* Try to disassociate if not already associated */
    r = AtCmd_WD();
    if (ATCMD_RESP_OK != r) continue;
    ConsoleLog("AtCmd_WD");
    /* Disable DHCP Client */
    r = AtCmd_NDHCP(0);
    if (ATCMD_RESP_OK != r) continue;
    ConsoleLog("AtCmd_NDHCP");
    /* Description: Set network paremeters statically  */
    r = AtCmd_NSET(UDPCLNT_IP, SUBNETMASK, GATEWAY_IP);
    if (ATCMD_RESP_OK != r) continue;
    ConsoleLog("AtCmd_NSET");
    /* Set WPA2 Passphrase */
    r = AtCmd_WPAPSK(AP_SSID, PASSPHRASE);
    if (ATCMD_RESP_OK != r) continue;
    ConsoleLog("AtCmd_WPAPSK");
    /* Associate with AP */
    r = AtCmd_WA(String(AP_SSID).c_str(), "", 0);
    if (ATCMD_RESP_OK != r) continue;
    ConsoleLog("AtCmd_WA");
    break;
  }

  bool served = false;
  while (1) {
    if (!served) {
      ConsoleLog("Start UDP Client");
      // Create UDP Client
      server_cid = gs2200.connectUDP(UDPSRVR_IP, UDPSRVR_PORT, LocalPort);
      ConsolePrintf("server_cid: %d \r\n", server_cid);
      if (server_cid == ATCMD_INVALID_CID) {
        continue;
      }
      served = true;
      break;
    } else {
      ConsoleLog("Start to send UDP Data");
      // Prepare for the next chunck of incoming data
      WiFi_InitESCBuffer();
    }
  }

  void *adder;
  MP.Send(msgid, &adder);           //準備完了をメインコアに通知
  MP.Recv(&msgid, &adder);          //メインコアの準備完了通知を受信
  MP.RecvTimeout(MP_RECV_POLLING);  //コア間通信ブロッキングからポーリングへ

  IntervalSet(dt);  //送信サイクル開始
}

void loop() {
  send_cycle();
}

SubCore2ソースコード

このプログラムは以下のライブラリを使用して作成されています。

1. <MP.h>:

   • GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照
   • 詳細: https://developer.sony.com/develop/spresense/
   • このライブラリの著作権: Copyright 2019 Sony Semiconductor Solutions Corporation.

2. <SoftwareSerial.h>:

   • Arduino標準ライブラリ
   • GNU Lesser General Public License (LGPL) v2.1に基づいて提供
   • 詳細: https://www.arduino.cc/en/Reference/SoftwareSerial

3. <MAVLink.h>:

   • MAVLinkプロトコルライブラリ
   • GNU General Public License (GPL) v3に基づいて提供
   • 詳細: https://mavlink.io/en/

ライセンス条項については、各公式サイトやソースコードの配布元をご参照ください。

Subcore2
#include <MP.h>
#include <SoftwareSerial.h>
#include <MAVLink.h>
#define CONSOLE_BAUDRATE 1000000

SoftwareSerial mySerial_mavlink(15, 6);
mavlink_message_t msg;
mavlink_status_t status;

union mavlink_data {
  struct {
    uint8_t battery_remaining;
    uint8_t rssi;
    uint8_t satellites_visible;
    uint8_t system_status;

    uint16_t load;
    uint16_t voltage_battery;
    uint16_t current_battery;
    uint16_t chan1_raw;
    uint16_t chan2_raw;
    uint16_t chan3_raw;
    uint16_t chan4_raw;
    uint16_t chan5_raw;
    uint16_t chan6_raw;
    uint16_t chan7_raw;
    uint16_t chan8_raw;
    uint16_t vel;
    uint16_t cog;

    int16_t temperature;

    uint32_t time_boot_ms;

    int32_t lat;
    int32_t lon;
    int32_t alt;

    float press_abs;
    float roll;
    float pitch;
    float yaw;
  };
  uint8_t bin[sizeof(uint8_t) * 4 + sizeof(uint16_t) * 13 + sizeof(int16_t) * 1 + sizeof(uint32_t) * 1 + sizeof(int32_t) * 3 + sizeof(float) * 4];
};
mavlink_data send_data;

int subcore = 1;
int8_t msgid = 0;
void setup() {
  Serial.begin(CONSOLE_BAUDRATE);
  MP.begin();
  MP.RecvTimeout(MP_RECV_BLOCKING);
  mySerial_mavlink.begin(38400);
  msgid = 0;
  void *adder;
  MP.Send(msgid, &adder);
  MP.Recv(&msgid, &adder);
  MP.RecvTimeout(MP_RECV_POLLING);
}

void loop() {
  while (mySerial_mavlink.available()) {
    if (mavlink_parse_char(MAVLINK_COMM_0, mySerial_mavlink.read(), &msg, &status)) {
      switch (msg.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT:
          mavlink_heartbeat_t heartbeat;
          mavlink_msg_heartbeat_decode(&msg, &heartbeat);
          send_data.system_status = heartbeat.system_status;
          break;
        case MAVLINK_MSG_ID_ATTITUDE:
          mavlink_attitude_t attitude;
          mavlink_msg_attitude_decode(&msg, &attitude);
          send_data.time_boot_ms = attitude.time_boot_ms;
          send_data.roll = attitude.roll;
          send_data.pitch = attitude.pitch;
          send_data.yaw = attitude.yaw;
          break;
        case MAVLINK_MSG_ID_SYS_STATUS:
          mavlink_sys_status_t sys_status;
          mavlink_msg_sys_status_decode(&msg, &sys_status);
          send_data.load = sys_status.load;
          send_data.voltage_battery = sys_status.voltage_battery;
          send_data.current_battery = sys_status.current_battery;
          send_data.battery_remaining = sys_status.battery_remaining;
          break;
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
          mavlink_rc_channels_raw_t rc_channels_raw;
          mavlink_msg_rc_channels_raw_decode(&msg, &rc_channels_raw);
          send_data.chan1_raw = rc_channels_raw.chan1_raw;
          send_data.chan2_raw = rc_channels_raw.chan2_raw;
          send_data.chan3_raw = rc_channels_raw.chan3_raw;
          send_data.chan4_raw = rc_channels_raw.chan4_raw;
          send_data.chan5_raw = rc_channels_raw.chan5_raw;
          send_data.chan6_raw = rc_channels_raw.chan6_raw;
          send_data.chan7_raw = rc_channels_raw.chan7_raw;
          send_data.chan8_raw = rc_channels_raw.chan8_raw;
          send_data.rssi = rc_channels_raw.rssi;
          break;
        case MAVLINK_MSG_ID_GPS_RAW_INT:
          mavlink_gps_raw_int_t gps_raw_int;
          mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
          send_data.lat = gps_raw_int.lat;
          send_data.lon = gps_raw_int.lon;
          send_data.alt = gps_raw_int.alt;
          send_data.vel = gps_raw_int.vel;
          send_data.cog = gps_raw_int.cog;
          send_data.satellites_visible = gps_raw_int.satellites_visible;
          break;
        case MAVLINK_MSG_ID_SCALED_PRESSURE:
          mavlink_scaled_pressure_t scaled_pressure;
          mavlink_msg_scaled_pressure_decode(&msg, &scaled_pressure);
          send_data.temperature = scaled_pressure.temperature;
          send_data.press_abs = scaled_pressure.press_abs;
          break;
        default:
          break;
      }
      msgid = 9;
      subcore = 1;
      MP.Send(msgid, &send_data, subcore);
    }
  }
}

表示器ソースコード

このコードは以下のライブラリを使用して作成されています。

1. socket, struct: Python標準ライブラリ（Python Software Foundation Licenseに基づいて提供）
2. OpenCV (cv2): BSD 3-Clause License（https://github.com/opencv/opencv/blob/4.4.0/LICENSE）
3. NumPy (numpy): BSD License（https://numpy.org/doc/stable/license.html）

各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。

udp_server
import socket
import cv2 #openCV
import numpy as np
from struct import *
M_SIZE = 65536#バッファサイズ
host = '0.0.0.0'
port = 10001
serv_address = (host, port)

#回転行列
def rpy2rot(rpy):
    yaw = rpy[2]
    pitch = rpy[1]
    roll = rpy[0]
    Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]])
    Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
    return Rz@Ry@Rx

def main():
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind(serv_address)
    frame_num = 0
    bytesimg = bytearray()
    struct_format = "BBBBHHHHHHHHHHHHHhIiiiffff"
    status_list = []
    status_list.append("Uninitialized system, state is unknown.")
    status_list.append("System is booting up.")
    status_list.append("System is calibrating and not flight-ready.")
    status_list.append("System is grounded and on standby. It can be launched any time.")
    status_list.append("System is active and might be already airborne. Motors are engaged.")
    status_list.append("System is in a non-normal flight mode (failsafe). It can however still navigate.")
    status_list.append("System is in a non-normal flight mode (failsafe). It lost control over parts or over the whole airframe. It is in mayday and going down.")
    status_list.append("System just initialized its power-down sequence, will shut down now.")
    status_list.append("System is terminating itself (failsafe or commanded).")

    fps = 15
    w = 640
    h = 480
    fourcc = cv2.VideoWriter_fourcc('m', 'p', '4', 'v')
    video = cv2.VideoWriter('video.mp4', fourcc, fps, (w, h))
    cv2.namedWindow('FPV',cv2.WINDOW_NORMAL)
    cv2.setWindowProperty('FPV',cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)

    with open('qvga_jpgqf40.bin', 'rb') as file:
        img_haeder = file.read()

    while True:

        rx_meesage, addr = sock.recvfrom(M_SIZE)
        frame_num = rx_meesage[-1]
        print(frame_num)
        if frame_num == 0 and len(bytesimg) > 0:
            # if bytesimg[0] == 0xff and bytesimg[1] == 0xd8:
            if bytesimg[0] == 0xff and bytesimg[1] == 0xda:
                bytesimg = img_haeder + bytesimg
                data = np.frombuffer(bytesimg, dtype=np.uint8)
                image = cv2.imdecode(data, cv2.IMREAD_COLOR)
                img_numpy_bgr=cv2.resize(image,None,fx=2,fy=2,interpolation=cv2.INTER_CUBIC)
                try:
                    
                    roll = unpack_data[23]
                    pitch = unpack_data[24]
                    yaw = unpack_data[25]*0

                    tx = 0
                    ty = 0
                    tz = 0
                    fx = 1000
                    fy = 1000
                    cx = img_numpy_bgr.shape[1]/2
                    cy = img_numpy_bgr.shape[0]/2
                    s = 0
                    
                    Rzyx = rpy2rot(np.array([pitch,yaw,roll])).T
                    RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]])        
                    K = np.array([[fx,s,cx],[0,fy,cy],[0,0,1]])

                    bar_length = img_numpy_bgr.shape[1]/4

                    XYZ = np.array([bar_length/2,0,fx,1])
                    PXYZ = K@RT@XYZ.T
                    ex1 = int(PXYZ[0]/PXYZ[2])
                    ey1 = int(PXYZ[1]/PXYZ[2])
                    XYZ = np.array([-bar_length/2,0,fx,1])
                    PXYZ = K@RT@XYZ.T
                    ex2 = int(PXYZ[0]/PXYZ[2])
                    ey2 = int(PXYZ[1]/PXYZ[2])

                    XYZ = np.array([0,bar_length/2,fx,1])
                    PXYZ = K@RT@XYZ.T
                    ex3 = int(PXYZ[0]/PXYZ[2])
                    ey3 = int(PXYZ[1]/PXYZ[2])
                    XYZ = np.array([0,-bar_length/2,fx,1])
                    PXYZ = K@RT@XYZ.T
                    ex4 = int(PXYZ[0]/PXYZ[2])
                    ey4 = int(PXYZ[1]/PXYZ[2])
                    
                    cv2.line(img_numpy_bgr,pt1=(ex1,ey1),pt2=(ex2,ey2),color=(0, 0, 255),thickness=2)
                    cv2.line(img_numpy_bgr,pt1=(ex3,ey3),pt2=(ex4,ey4),color=(0, 0, 255),thickness=2)

                    font = cv2.FONT_HERSHEY_SIMPLEX
                    cv2.putText(img_numpy_bgr,text="bat[%]:" + str(unpack_data[0]),org=(10,20), fontFace=font,fontScale=0.5,color=(0,int(255*unpack_data[0]/100),int(255*(1.0-unpack_data[0]/100))),thickness=1,lineType=cv2.LINE_AA)
                    cv2.putText(img_numpy_bgr,text="sat[-]:" + str(unpack_data[2]),org=(10,40), fontFace=font,fontScale=0.5,color=(0,int(0),int(0)),thickness=1,lineType=cv2.LINE_AA)
                    cv2.putText(img_numpy_bgr,text="status:" + status_list[unpack_data[3]],org=(10,60), fontFace=font,fontScale=0.5,color=(0,int(0),int(0)),thickness=1,lineType=cv2.LINE_AA)

                    cv2.imshow('FPV', img_numpy_bgr)
                    video.write(img_numpy_bgr) 
                except NameError:
                    error_msg = "error"
                    print(error_msg)
            bytesimg = bytearray()
        if frame_num == 0:
            bytesbuf = bytearray()
            bytesbuf = rx_meesage[:-1]
            unpack_data = unpack(struct_format, bytesbuf)
        else:
            bytesbuf = bytearray()
            bytesbuf = rx_meesage[:-1]
            bytesimg = bytesimg + bytesbuf

        if cv2.waitKey(1) & 0xFF == ord('q'):
            sock.close()
            cv2.destroyAllWindows()
            break

if __name__ == '__main__':
    main()