PCからシリアル通信したメッセージを中継するNefryBTからTelloを操作するメモ

Posted on 2018-07-31

PCからシリアル通信したメッセージを中継するNefryBTからTelloを操作するメモです。

やりたいこと

こちらの記事で、Node-REDとトイドローンTelloを連携するナレッジを書いたんですが、悩ましいところがあります。

というのも、TelloをWi-Fiで接続してUDPで操作する場合、命令をするIoT機器がWi-Fiで縛られてしまいます。さらに、そのIoT機器が外部操作を受け入れられない限り、ネットワークから孤立しまうところが悩ましいところなんです。

Raspberry Piは幸いにも、有線でのLANと無線LANでの両方のネットワークを受け入れられるので、その制約を超えることが出来ますが、もう少し他の方法も模索したいという話。

今回は、PCからシリアル通信したメッセージを中継するNefryBTを作り、Telloを操作するようにします。

NefryBTソースコード

NefryBTソースコードはこちらです。

#include <Nefry.h>
#include <NefryDisplay.h>
#include <WiFiUDP.h>

// Nefryメッセージ用Buffer変数
char NefryTextBuffer[50];
// TELLO設定
const char* TELLO_IP = "192.168.10.1";
const int PORT = 8889;
// 離陸着flag
int takeoff_flag = 1;

WiFiUDP Udp;
char packetBuffer[255];
String message = "";

void setup() {
  Serial.begin(115200);
  // ボタンの有効化
  Nefry.enableSW();
  // UDP
  Udp.begin(PORT);
  Serial.println("start");
}

void loop() {
  if (Serial.available() > 0){
    // 改行で区切る
    String str = Serial.readStringUntil('\n');
    // Stringから char*の変換
    char* sendstr = strdup(str.c_str());
    telloMessage(sendstr);
  } else {
    if (Nefry.readSW()) {
      if(takeoff_flag == 1){
        // チェック用、1度ボタンを押すと離陸
        telloMessage("command");
        telloMessage("takeoff");
        takeoff_flag = 0;
      } else {
        // チェック用、もう一度ボタンを押すと着陸
        telloMessage("command");
        telloMessage("land");
        takeoff_flag = 1;
      }

      Nefry.ndelay(1000);
    }
  }
}

// Telloからのレスポンスを確認する関数
String listenMessage() {
  int packetSize = Udp.parsePacket();
  if (packetSize) {
    Serial.print("Received packet of size ");
    Serial.println(packetSize);
    Serial.print("From ");
    IPAddress remoteIp = Udp.remoteIP();
    Serial.print(remoteIp);
    Serial.print(", port ");
    Serial.println(Udp.remotePort());

    // read the packet into packetBufffer
    int len = Udp.read(packetBuffer, 255);
    if (len > 0) {
      packetBuffer[len] = 0;
    }
    Serial.println("Contents:");
    Serial.println(packetBuffer);
  }
  // this only works as tello's API doesn't return responses greater than 255 char
  return (char*) packetBuffer;
}

// UDPでTelloに命令を送る関数
void sendMessage(char* ReplyBuffer) {
  Udp.beginPacket(TELLO_IP, PORT);
  Udp.printf(ReplyBuffer);
  Udp.endPacket();
}

// Telloにデータを送受信する関数
void telloMessage( char* command ) {
  sendMessage(command);
  message = listenMessage();
  char* messtr = strdup(message.c_str());
  displayMessage(command,messtr);
}

// 表示
void displayMessage( char* command , char* message ) {
  sprintf(NefryTextBuffer,"command: %s",command);
  NefryDisplay.print(NefryTextBuffer);
  Serial.println(NefryTextBuffer);
  sprintf(NefryTextBuffer,"message: %s",message);
  NefryDisplay.print(NefryTextBuffer);
  Serial.println(NefryTextBuffer);
}
[/html]</div>

こちらをNefryBTに書き込みます。

### 苦労した点：シリアルで文字列受信させること

いままで、Serial.readで1文字ずつ読んでいたのですが、かなり大変だったところを、以下の記事で救われました。

<a href="http://ch.nicovideo.jp/yugata/blomaga/ar1177727" target="_blank">【prog】Arduino・シリアルで文字列受信するには:ゆうがたの「特にコレと言って」 - ブロマガ</a>

> 引っ張るのも嫌なので、先に結論を書きます。
　Serial.read()でもSerial.readString()でもなく、
　Serial.readStringUntil(char)を使え！

とのことで、readStringUntilで取得しやすく！

### 苦労した点：Stringから char*の変換

しかし、readStringUntilを返すのがStringでして、Stringから char*の変換に手間取りました。

<a href="https://stackoverflow.com/questions/25549562/how-to-convert-const-char-to-char-in-c" target="_blank">pointers - How to convert const char* to char* in C? - Stack Overflow</a>

<div>[html]
    // Stringから char*の変換
    char* sendstr = strdup(str.c_str());
    sendMessage(sendstr);
    message = listenMessage();