サンプルプログラム公開

GPS・QZSSロボットカーコンテスト2020で使用した測位とWaypoint算出に関するソースコードを参考用にUpします。
本ソースコードは「測位航法学会特製 F9P搭載受信機」にも対応しているはずです。

最適ではないのですが、参考になる方もいるかと思い公開に踏み切りました。 もちろん無保証なのと、これによる問題障害については一切責任は負えないので、そのあたりを理解してみてもらえば幸いです。 また、本業が忙しく、コメント・質問などに、答えられない場合があります。


2019年大会はFix率が悪く、走行テストが難航し2回目の走行をキャンセルせざるを得なかったのですが、今年は参考までに以下の対策を講じてみた。 (どれが効果的かというのは感触です。)

-F9PのFarmwareのUpgrade
これはかなり効果があったと思います。
-QZSSの無効化
これも効果があったように感じますが、FarmのUpdate後にL1Sに関する記事を読みL1Sを無効化して再登録。 Fix率は下がらないことを確認した。 L1Sの無効化が聞いたのかFarmware Updateが効いたのかは不明。
-アンテナ背面の銅箔テープによるマルチパス防止
これは気持ちの問題だった気がする。
-ntrip Serverとのアクセス手法
これは効果大だった。制御周期も早いので、制御の合間に数文字程度を読み取って制御側の処理をやっていたが、メッセージ終了までは制御は行わず一気に処理を行った。
-F9P更新周期
F9Pはカタログスペック上20Hzとなっており、51ms(50msが定義できなかった)で動作させた。Logを見るとデータが変更しないときもあるが、最速で見るれ可能性が高いと想定し使ったが、Fixしたのは1-2度のみですぐに設定を戻した。(100mS~300ms)

そのほか2019年では悩んでいたBluetooth通信であったが、ESP32 DevkitをV4にすることで同じソースコードで動作した。試しにV2に戻すと動作しない。 昨年はこれでだいぶん時間を食ってしまったが。 ただ、PC側は毎回削除し、登録している。 この辺はかなり改善の余地がある。 また、Bluetoothを使っていると、たまに処理が止まったり遅くなったりすることがある。情報取得時以外はPC側から接続しないで使っていた。

—–
本ソースコードは無保証の条件で、無償提供し、個人・商用を問わず利用は自由です。ただし、内部で使用されているライブラリなどは、そのライセンス条項に従ってください。

Sample.zip

—–

GPSモジュールから取得できる電文について

ここで使用しているGPSモジュールはubloxのNEO-M8Nである。
データシートを見ると、「NMEA 0183, version 4.0 (V2.3 or V4.1 configurable)」と記載がある。NMEAの電文を利用するだけで、緯度、経度、進行方向、対地速度などは利用できる。ただし、ある程度の移動速度が出ていないと進行方向が出てこないので、自車の方位は地磁気コンパスなどを用いるほうがよいであろう。
地磁気コンパスについても、そのうち記載できれば良いと思っている。
詳細は「NMEA 0183 フォーマット」として検索していただければよい。

参考までに、持っているNEO-M8Nのファームウェアのバージョンは2.01であった。u-bloxのホームページには3.01のファームウェアも公開されており、アップデートすることは可能である。ただ、やみくもにアップグレードするのも注意する必要がある。3.01では機能アップもされていることながら、生データが出力しなくなっている。ファームウェアのイメージがあれば、2.01、3.01を行ったり来たりできるので大丈夫である。

GPSモジュールから来たデータを変数に格納する

前回のプログラムは、GPSモジュールから来た情報をそのままUSBのシリアルに送っていただけである。
ただの通信テストに用いる場合に用いる手段である。
実際にはGPSモジュールから読み込んだデータを加工する必要があるので、変数に代入しておかないとならない。
ここでトラブルケースが多いので記載しておく。
手法としてはいくつかあるので、一つのサンプルとして受け止めてもらえればありがたい。
事実、大会で実際に使用したプログラムとは手法が違う。なぜ違う手法を使ったのか考えてもらいたい。

さて、プログラムの説明である。

Loopの中でBufferを監視しており、Bufferに何か入ったらGPS_Readという関数を呼び出している。
関数の中では、改行文字(アスキーコード 0x0A:Line feed)を検出するまでWhileループしている。
ここで再度Bufferが空かどうか確認しているのは、Bufferが空な状態で読んでしまうと、-1(0xFF)を受信してしまうことを恐れている。
Bufferが空でなければ1文字読みだして返り値用の変数に順次1文字づつ格納している。
最後に改行文字(今回はLine Feed文字)が来た後に、0x0を書き込み文字列の終わりであることを示している。この0x0が入った以降は、ゴミが入っていても文字列としては無視されるので、わざわざ消す必要がない。
文字列に入った文字はシリアルモニタに出力している。
この後の必要部分の抜き出し等は純然なプログラミングの世界となるので、他のWebや文献を参考にしてほしい。

#include <SoftwareSerial.h> // Software Serial library

SoftwareSerial GPS_Serial(5,6); // 5:RXD, 6:TXD

void GPS_Read(char *SoftSerialBuffer){
   int i = 0; // Move first character position
   char read_ch;
   while (1) {
      if (GPS_Serial.available()) { // Check empty buffer
         read_ch = GPS_Serial.read(); // Read one character from buffer
         SoftSerialBuffer[i] = read_ch; // Copy reading character to return-variable
         if (read_ch == 10) break; 
         i++;
      }
   }
   SoftSerialBuffer[i] = '\0'; // Set end-of-string as '\0' 
}

void setup() {  //***** set serial port speed (bps)
   Serial.begin(115200); // USB 
   GPS_Serial.begin(9600); // Software Serial setting for GPS module
}

void loop() {
   char GPS_Data_Raw[255]; // Buffer size is 254 characters. 
   Serial.println("Loop"); // Eye catcher
   if (GPS_Serial.available()) { // Check empty buffer
      GPS_Read(GPS_Data_Raw); // Call GPS_Read function routine
      Serial.println(GPS_Data_Raw);
   }
}

 

GPSモジュールとArduinoとの通信

Arduino UNOにはシリアル通信用のピンが1セット準備されているのだが、これは、シリアル通信変換回路を通じてUSBコネクタに通じているようである。
余談になるが、2017年の大会中の出来事で、デバッグ中にUSBケーブルをつけたままロボットカーが走り出してしまいUSBポートを物理的に破壊してしまった。(Arduino Unoではないが。)USBポートの破壊による回路欠損はその場ではんだごてを使って応急処置ができたものの、USBコネクタは使い物にならなくなってしまった。その時は、パソコンとシリアル端子を接続することで無事にダウンロードをすることができ九死に一生を得た。

UNOの場合は、シリアル通信が1セットだけである。しかし、実際のアプリ作成には、Arduino IDEのシリアルモニタでプログラムの動作を確認しながらGPSモジュールの通信を行うために、複数のシリアル通信を用いる必要がある。
複数のシリアル端子を持っているArduino MEGAを使うという手もあるが、Arduinoは入出力に使う端子を利用してシリアル通信を利用できる。
ソフトウェアシリアルと呼ばれており、ライブラリが準備されている。

作成したプログラムのデバッグ用にはUSB経由のシリアル通信を用い、GPSモジュールとの通信にはソフトウェアシリアルを用いるということである。下記はArduinoとGPSモジュールを接続してArduino IDEのシリアルモニタで観測した様子である。

配線は、Arduino UNIの5V,GNDと5番、6番の各ピンを使用した。Arduino IDEのスケッチは下記のように組んでみた。ソフトウェアシリアルを組み込み、GPS_Serialと名前をつけて、5番、6番のピンを指定した。
その後、シリアルモニタ用には115200kbpsの通信速度を、GPSモジュール用のGPS_Serialには9600bpsの通信速度を定義した。プログラムとしては、ただ単にソフトウェアシリアルのバッファにたまった電文を読み(read)、USBを経由するシリアルに書き込んで(write)しているだけである。
これでとりあえずGPSモジュールからの信号もArduinoの中に取り入れたことになる。

#include <SoftwareSerial.h> // Software Serial library

SoftwareSerial GPS_Serial(5,6); // 5:RXD, 6:TXD

void setup() {
 //***** set serial port speed (bps)
 Serial.begin(115200); // USB
 GPS_Serial.begin(9600); // Software Serial setting for GPS module
}

void loop() {
 if (GPS_Serial.available()) {
   Serial.write(GPS_Serial.read());
 }
}


GPS受信データ

GPSからの受信状況は以下に示す。
GPSアンテナからは東側の空しか見えず、非常に悪い環境なのがわかる。

これを見ると、片側が受信できている様子がなんとなくわかる。
また、この状態でGPSの位置を見ると結構ずれており、このままではあまり使えない。
軸のレンジが少しわかりにくいのだが、一番小さい円から2.5m刻みである。
なので、一番外側の円が10mの誤差となる。

これをできるだけ精度を高めるためには、いくつかのノウハウがある。
まずは、全天を見張らせるところで実験してみると、結構そのまま使えるデータになることもある。いろいろとGPSモジュールの設定を変えるなどして試してみるとよい。上記はGPSモジュールを固定して長時間データ収集した結果である。

U-Centerとの接続

設定用ソフトウェア(Windows版GNSS評価ソフトウェア)であるu-center Windows (https://www.u-blox.com)と接続してみる。
先に使った設定では、GPSモジュールはCOM3に接続され、9600bpsの通信速度になっている。
U-Centerは起動すると、デフォルトの通信速度が9600bpsであり、COM Portも表示してくれるので、Com Portだけを設定すればよい。
下の図の矢印のところあたりでPortと通信速度が設定できる。
すると、通信を始める。

あとは、U-Centerのマニュアルを見ながらいろいろと試してもらいたい。
ちなみに、GPSデータは以下の様に出力される。位置情報は黒塗りさせてもらっているが、雰囲気はわかっていただけたと思う。

GPSモジュールとPCを接続

実際にGPSモジュールとPCを接続してみる。
多くの方がそうだと思うが、PCとGPSモジュールを接続した状態で、GPS信号をそのまま受信できる環境を持っている人は多くないと思われる。
窓の近くにPCと接続されたGPSモジュールを持っていくことになり、これが結構面倒である。
たまたま、自宅に古い電話線の余りが15mほどあったので、その電話線を延長ケーブルとして使っている。
実は、こんな小さい工夫がちょっとした実験に役立っている。
電話線を使ったのは、細くて柔らかいからであり、実際には4本以上の配線があればよいので、LANケーブルを使ってしまってもよい。
信号線にはツイストペアが良いという話は確かにあるが、この実験程度では気にしなくてよい。

もちろん端末処理は行い、GPSモジュール用のコネクタも作成した。

ちなみに、このGPSモジュールで使われているコネクタの種類は、
DF13シリーズ 1.25mmピッチ である。 結構探すのに苦労した。
目的が違う基盤なのだが、たまたまピッチがあったので代用させてもらった。

このUSB-シリアル変換は5Vの出力があるので、GPSモジュールはこの5V,GNDを赤黒で接続。
(ここは、大体想像がつくのだが、ケーブル配線については、自己責任で行ってください。)

そのあと、TXDとRXD、RXDとTXDを接続する。
PCは端末側であり、GPSモジュールはセンサー側であるが、これも一つの端末として考えるケースが多い。
その場合、端末と端末を接続するために、TXDとRXDを接続。 RXDとTXDを接続する。
一般にクロスといわれる接続方法である。
参考までに、端末とモデムなどの延長機器を接続する場合は、TXDとTXD。 すなわちストレートといわれる接続方法である。
この延長機器には、ネットワークに接続する機器であったり、長距離の延長ケーブルを用いるための信号変換だったりする。
ただ、世の中にはDTE機器、DCE機器があやふやなものも多く、筆者はあまり厳密に考えないで試行錯誤を繰り返して決めている。

さて、今回は入手してから一度も設定変更をしていないGPSモジュールを用いた。
それに、PC側で用いるTera Termもインストールしなおしてそのままの設定。

配線も終わり、USB-Serialコンバータを接続。 動作を示す電源のLEDが付けば、USB-Serial側は大丈夫。
GPSもモジュールを見てみて、どこかにLEDなどが点灯していれば大丈夫。
もちろん、GPSモジュールの種類に左右されるのは言うまでもない。
ダメなときは、まず電源を確認をすることとなる。

このGPSモジュールは毎秒データを送信してくれるので、USB-シリアル変換も毎秒通信を示すLEDが点滅していれば大丈夫。
もしLEDが点滅していなければ、TXD/RXDの配線を入れ替えてみることとなる。

次はPC側。 USB-Serialコンバータがどのポートに割りついたかを把握するのだが、ここで把握しなくとも、通信ソフトとなるTera Termで想像できることが多い。
なので、まず、Tera Termを起動する。起動画面でTCP/IPとシリアルの選択ができるのだが、USB-シリアル変換が正しく認識できていないとこのシリアルの選択ができない。
認識されている場合、下図のようになるので、シリアルを選択する。


その段階でデータが表示し始めた。
今回は、GPSモジュールとTera Termの通信設定が一致していたためにすぐに通信が開始した。
その通信設定とは、
通信速度:9600bps
データ長:8bit
パリティ検査:なし
ストップビット:1bit
フロー制御:なし
である。

設定画面の後ろに$GN….と表示されているのがひっさいにGPSモジュールからきた信号である。
“,”が多いのは、GPSモジュールは生きているが、GPS信号は受信できていないことを示す。

この状態が確認できたら、GPSモジュールを空が見えるところに持っていってほしい。
少し、場合によっては数分程度待つと数字がたくさん表示されてくる。

PCとGPSモジュールの接続方法

ここではGPS Moduleについて記載する。
GPSモジュールはたくさんのベンダーから出されているので、各製品にあった使い方が必要である。
いきなりGPSモジュールをArduinoに接続して使うのではなく、
まずGPSモジュールとPCをつなげて動作確認や各種の設定を行うことを強く推奨する。

PCで使うためには、いくつかの環境が必要である。
筆者の環境を参考までに記載すると、

GPS Module(アンテナ込 パッケージ品)
NEO-M8N (https://www.u-blox.com)

設定用ソフトウェア(Windows版GNSS評価ソフトウェア)
u-center Windows (https://www.u-blox.com)

USB-TTLコンバータ
CP2102 (製品名不明 300-400円程度で仕入れたもの)

である。

接続のイメージは以下である。


筆者は、複数のGPSモジュールを持っているが、配線色がバラバラなので、
注意してもらいたい。

4本配線があるコネクタは
各々が、+5V,GND,TXD,RXDとなっていた。
これを接続すればGPSが動作する。

一方、2本配線があるコネクタは
GPSモジュールに内蔵されているI2Cの地磁気コンパスであり、
ここからのは、SDA,SCLである。

ステアリングサーボを動かす

ステアリングサーボはスピードコントローラと同じなのでプログラムの変更はほとんどないので、細かい説明は省略する。

サーボを動作させるためのSteering_Valの値は、以下のとおりである。
50:ステアリングを左に切った状態
90:中立
130:ステアリングを右に切った状態

また、シリアルモニタからの入力で3,4を入力することにより、Steering_Valの値が上下するようになっている。

#include <Servo.h> // Built-in Servo library
Servo servo[2]; // create servo object for speed controller and steering servo
int SerialInputValue; // Inputed value from serial monitor for debug

void setup() {

//***** set serial port speed (bps)
 Serial.begin(115200);

//***** set servo pin servo[0]..servo[1]
 servo[0].attach(9); // Pin 9 is for Stering Servo
 servo[1].attach(10); // Pin 10 is for Speed Controller

}

void loop() {
 static int Steering_Val = 90; // 50-90-130 Steering value (90:center)
 static int Motor_Val = 94; // 70-92 Speeding Value (70 is fast)

 SerialRead(); // read inputted numeric key
 if (SerialInputValue ==3 && Steering_Val >=52){ 
 Steering_Val -= 2; // '3' is inputted from serial monitor
 }
 if (SerialInputValue ==4 && Steering_Val <=128){
 Steering_Val += 2; // '4' is inputted from serial monitor
 }
 if (SerialInputValue ==1 && Motor_Val >=72){ //73
 Motor_Val -= 2; // '1' is inputted from serial monitor
 }
 if (SerialInputValue ==2 && Motor_Val <=100){ //83
 Motor_Val += 2; // '2' is inputted from serial monitor
 }

 servo[0].write(Steering_Val); // Servo output (Steering)
 Serial.print("Steering Value=");
 Serial.print(Steering_Val);
 
 servo[1].write(Motor_Val); // Servo output (Speeding)
 Serial.print(" Motor Speed=");
 Serial.println(Motor_Val);
}

下記は実験しているときの写真である。
ArduinoからはGNDと9Pin,10Pinの③本だけが出ている。それらがスピードコントローラやステアリングサーボと接続されている。

配線図としては以下となる。

スピードコントローラに与える値

スピードコントローラに与える値はどうすればよいか?
具体的には、
servo[1].write(Motor_Val);
のMotor_Valの値である。 各々実験してもらうことを前提に、このモータとスピードコントローラのケースを記載しておく。

69以下 最高速
70-92 スピードが段階的に変化(70側が高速、90側が低速)
93-174 停止
175 以上 最高速 ←正しく制御できていない