初版

SpresenseでGNSSから速度計を動かす

製作品

# 何がしたいか [以前の記事](https://elchika.com/article/d4b69ae0-d6d2-4c7a-978e-ebd0a2681b42/)で速度計を動かしました。 でも、動かしただけでは物足りない！ということで今回は実際の速度にリアルタイムに追従して表示することを目標に開発をしてみることにします。 # どうやって実現するか 速度計の駆動回路は別で作ったものを用いて、GNSSで速度を取得する部分はSpresenseを用います。 簡単に図にすると次通りです。  ## 方針 - 速度計を動かす - GNSSから速度を取得する - 制御基板と通信を介して制御する - PlatformIOのArduino環境でコーディング ## 環境 - Spresense - メインボード - 拡張ボード - 速度計 - 阪急で使われてたやつ - 既存の制御基板 Spresenseのメインボードや拡張ボードはモニター品を提供していただきました、ありがとうございます:bow: # やってみる Spresenseから直接出力を行うのは厳しそうだったため、既存の制御基板を通して速度計を制御することにしました。 Spresenseと制御基板の間はI2Cで通信を行い、速度計は0-140km/hなので送信されたデータのうち最下位1バイトのみを採用して、その値を丸ごと速度計の指示値とすることにしました。 I2CはSpresenseをマスター、制御基板のesp32をスレーブとして、適当に実装しました。 その辺の実装をライブラリが適当にやってくれるのは気分がいいですね。 ## ハードの構成 制御基板は速度計に取り付いているので、制御基板への電源供給と通信だけで速度計を動かすことができます。 簡単に運んで使用するために、電源一本だけで動くことを目標に構成を考えます。 Spresense側のI2Cは拡張ボードの右上にあるD14, D15を使います。 制御基板には12Vから5Vに電圧を落とすコンバーターが乗ってるので、12Vをここに供給してSpresenseはコンバーターが出力する5Vをもらうことにします。 自動車なんかだと12Vがシガーからとれるので簡単ですね！ （セルモーターやオルタネーターがつながっていてきれいな12Vじゃないところにつなぐのはやめておいた方がよさそうですが...） 電源や通信回りを簡単な図にすると次の通りです。  ## Spresense側の実装 Spresense拡張ボードはレベルシフト回路が入っていて、常に電源にプルアップされているのでシビアな環境でなければI2Cのプルアップを省略できます。 わざわざ外部でプルアップするのは面倒なのでやるとしても内蔵プルアップだとは思いますが、今回はレベルシフト回路に任せて省略します。 ### コード ほとんどサンプル[^1]のパクリですが、追加や修正をしたポイントだけ触れておきます。 - GNSS更新周期 - ライブラリを少しいじって実装（詳しくは下で） - 1秒では物足りないので0.25秒に - ベストエフォートらしいのでタイミングによっては1秒に3回になるかも - GNSS設定 - デフォルトではGPSのみ - 日本ではみちびき（QZSS）が利用可能 - みちびきの衛星測位サービス（GPSと同種の信号:L1C/A）を使用 - みちびきのサブメータ級測位補強サービス（ほかの信号と組み合わせて誤差1mで測位を行うことができる信号:L1S）を使用 - ログとエラーの整備 - ログの類は全部`MPLog` - エラーの類は全部`MPERR` - サブコアへの通知 - LCD制御用のサブコアへ位置情報を送る - LCDをつければサブコアからLCDを制御できる - 実験用に使ってたレガシーコード ```cpp:main.cpp #ifdef SUBCORE #error "Core selection is wrong!!" #endif #include <Arduino.h> #include <MP.h> #include <GNSS.h> #include <Wire.h> /* * GNSS */ #define STRING_BUFFER_SIZE 128 /**< %Buffer size */ #define RESTART_CYCLE (60 * 5) /**< positioning test term */ static SpGnss Gnss; /**< SpGnss object */ /** * @enum ParamSat * @brief Satellite system */ enum ParamSat { eSatGps, /**< GPS World wide coverage */ eSatGlonass, /**< GLONASS World wide coverage */ eSatGpsSbas, /**< GPS+SBAS North America */ eSatGpsGlonass, /**< GPS+Glonass World wide coverage */ eSatGpsBeidou, /**< GPS+BeiDou World wide coverage */ eSatGpsGalileo, /**< GPS+Galileo World wide coverage */ eSatGpsQz1c, /**< GPS+QZSS_L1CA East Asia & Oceania */ eSatGpsGlonassQz1c, /**< GPS+Glonass+QZSS_L1CA East Asia & Oceania */ eSatGpsBeidouQz1c, /**< GPS+BeiDou+QZSS_L1CA East Asia & Oceania */ eSatGpsGalileoQz1c, /**< GPS+Galileo+QZSS_L1CA East Asia & Oceania */ eSatGpsQz1cQz1S, /**< GPS+QZSS_L1CA+QZSS_L1S Japan */ }; /* Set this parameter depending on your current region. */ static enum ParamSat satType = eSatGpsQz1cQz1S; ... /** * @brief Activate GNSS device and start positioning. */ void setup() { /* put your setup code here, to run once: */ int error_flag = 0; /* Set serial baudrate. */ Serial.begin(115200); while (!Serial); MPLog("initialization mainCore\n"); /* Launch SubCore */ MPLog("Launch SubCore\n"); int ret = MP.begin(1); if (ret < 0) { MPERR("MP.begin error = %d\n", ret); } /* Wait HW initialization done. */ sleep(3); /* Turn on all LED:Setup start. */ ledOn(PIN_LED0); ledOn(PIN_LED1); ledOn(PIN_LED2); ledOn(PIN_LED3); /* Set Debug mode to Info */ Gnss.setDebugMode(PrintInfo); int result; /* Activate GNSS device */ result = Gnss.begin(); if (result != 0) { MPERR("Gnss begin error!!\n"); error_flag = 1; } else { /* Setup GNSS * It is possible to setup up to two GNSS satellites systems. * Depending on your location you can improve your accuracy by selecting different GNSS system than the GPS system. * See: https://developer.sony.com/develop/spresense/developer-tools/get-started-using-nuttx/nuttx-developer-guide#_gnss * for detailed information. */ switch (satType) { case eSatGps: Gnss.select(GPS); break; case eSatGpsSbas: Gnss.select(GPS); Gnss.select(SBAS); break; case eSatGlonass: Gnss.select(GLONASS); break; case eSatGpsGlonass: Gnss.select(GPS); Gnss.select(GLONASS); break; case eSatGpsBeidou: Gnss.select(GPS); Gnss.select(BEIDOU); break; case eSatGpsGalileo: Gnss.select(GPS); Gnss.select(GALILEO); break; case eSatGpsQz1c: Gnss.select(GPS); Gnss.select(QZ_L1CA); break; case eSatGpsQz1cQz1S: Gnss.select(GPS); Gnss.select(QZ_L1CA); Gnss.select(QZ_L1S); break; case eSatGpsBeidouQz1c: Gnss.select(GPS); Gnss.select(BEIDOU); Gnss.select(QZ_L1CA); break; case eSatGpsGalileoQz1c: Gnss.select(GPS); Gnss.select(GALILEO); Gnss.select(QZ_L1CA); break; case eSatGpsGlonassQz1c: default: Gnss.select(GPS); Gnss.select(GLONASS); Gnss.select(QZ_L1CA); break; } /* Set the GNSS operation interval. */ result = Gnss.setInterval(250); if (result != 0) { MPERR("Gnss set interval error!!\n"); error_flag = 1; } else { MPLog("Gnss set interval OK\n"); } /* Start positioning */ result = Gnss.start(COLD_START); if (result != 0) { MPERR("Gnss start error!!\n"); error_flag = 1; } else { MPLog("Gnss setup OK\n"); } } /* Start 1PSS output to PIN_D02 */ Gnss.start1PPS(); /* Setup I2C */ Wire.begin(); /* Turn off all LED:Setup done. */ ledOff(PIN_LED0); ledOff(PIN_LED1); ledOff(PIN_LED2); ledOff(PIN_LED3); /* Set error LED. */ if (error_flag == 1) { Led_isError(true); exit(0); } } ... /** * @brief %Print position information and satellite condition. * * @details When the loop count reaches the RESTART_CYCLE value, GNSS device is * restarted. */ void loop() { /* put your main code here, to run repeatedly: */ static int LoopCount = 0; static int LastPrintMin = 0; /* Blink LED. */ Led_isActive(); /* Check update. */ if (Gnss.waitUpdate(-1)) { /* Get NaviData. */ SpNavData NavData; Gnss.getNavData(&NavData); /* Set posfix LED. */ bool LedSet = (NavData.posDataExist && (NavData.posFixMode != FixInvalid)); Led_isPosfix(LedSet); /* Print satellite information every minute. */ if (NavData.time.minute != LastPrintMin) { print_condition(&NavData); LastPrintMin = NavData.time.minute; } delay(10); /* Print position information. */ print_pos(&NavData); /* Send Speed to meter */ Wire.beginTransmission(0x00); Wire.write(static_cast<uint8_t>(NavData.velocity*3.6)); Wire.endTransmission(); /* Send Speed to SubCore */ int8_t sndid = 0; int subcore = 1; uint32_t snddata = static_cast<uint32_t>(NavData.velocity*360); int ret = MP.Send(sndid, snddata, subcore); MPLog("Send: id=%d data=%lu\n", sndid, snddata); // sndid = (sndid != 127)? (sndid+1): 0; if (ret < 0) { MPERR("MP.Send error = %d\n", ret); } } else { /* Not update. */ MPLog("data not update\n"); } /* Check loop count. */ LoopCount++; if (LoopCount >= RESTART_CYCLE) { int error_flag = 0; /* Turn off LED0 */ ledOff(PIN_LED0); /* Set posfix LED. */ Led_isPosfix(false); /* Restart GNSS. */ if (Gnss.stop() != 0) { MPERR("Gnss stop error!!\n"); error_flag = 1; } else if (Gnss.end() != 0) { MPERR("Gnss end error!!\n"); error_flag = 1; } else { MPLog("Gnss stop OK.\n"); } if (Gnss.begin() != 0) { MPERR("Gnss begin error!!\n"); error_flag = 1; } else if (Gnss.start(HOT_START) != 0) { MPERR("Gnss start error!!\n"); error_flag = 1; } else { MPLog("Gnss restart OK.\n"); } LoopCount = 0; /* Set error LED. */ if (error_flag == 1) { Led_isError(true); exit(0); } } } ``` ### GNSSライブラリの変更 GNSSの更新周期が1秒では物足りないので変更します。 変更自体は`SpGnss::setInterval(long interval)`でできますが、引数の型が`long`でしかも単位が秒です。 そこで、ライブラリのコードを改変してミリ秒単位で設定できるようにします。 16行目は元は`setdata.cycle = interval * 1000;`だったところを、`setdata.cycle = interval;// * 1000;`とコメントアウト。 ここで引数の型が`float`か1000倍が元からなければ楽だったんですがね... ```cpp:lib/GNSS/GNSS.cpp（変更後） ... /** * @brief Set the pos interval time * @details Set interval of POS operation. * @param [in] Interval time[sec] * @return 0 if success, -1 if failure */ int SpGnss::setInterval(long interval) { int ret; struct cxd56_gnss_ope_mode_param_s setdata; /* Set parameter. */ setdata.mode = 1; setdata.cycle = interval;// * 1000; /* Call ioctl. */ ret = ioctl(fd_, CXD56_GNSS_IOCTL_SET_OPE_MODE, (unsigned long)&setdata); if (ret < OK) { PRINT_E("SpGnss E: Failed to set Interval\n"); } return ret; } ... ``` ## 制御基板側の実装 - esp32はI2Cスレーブとして動かす - 速度計の制御に関する部分は既存のコードを流用 ### コード コードをドーン！！ ```cpp:main.cpp #include <Arduino.h> #include "meter.h" const int address = 0x00; TwoWire slave(1); void receiveEvent(int); void requestEvent(void); void setup() { Serial.begin(115200); delay(1000); slave.begin(address, 33, 32, 0U); slave.onReceive(receiveEvent); slave.onRequest(requestEvent); delay(1000); } void loop() { delay(1000); } /* ----- for i2c slave ----- */ // 受信時のイベント void receiveEvent(int howMany) { if( howMany == 0 ) { return; } Serial.print("receiveEvent: "); while( 1 < slave.available() ) { // 最後の1バイト以外を受信 Serial.print(slave.read()); // 読みだした1バイトをシリアル出力 Serial.print(" "); } int x = slave.read(); // 最後の1バイトを読み出し writeSpeed(x); // 速度計の速度を設定 Serial.println(x); // 最後の1バイトをシリアル出力 } // リクエスト受信時？のイベント void requestEvent(void) { Serial.println("requestEvent: "); slave.write("res\n"); // 適当に応答を返す } ``` # まとめ 動いた！！ 実際に速度計を積んで自動車で走ってみました。 @[youtube](https://youtu.be/k1UMcsOBcf0) とりあえずかなりそれっぽく動いたので今回は良しとします... 課題があるとすれば、更新頻度が少し低いこと室内やトンネル内では速度がわからないことでしょうか。 ## 躓きポイント @[youtube](https://youtu.be/T5iUVKqyvdY) この動画では30-50km/h程度で走行していますが、実際の速度とは大きくかけ離れた値が表示されています。 停車するとなんとなく0に近づくので動作自体はそれっぽいですが、速度が上がるにつれ真の値との誤差が大きくなっていくことがわかります。 この現象の原因は、出力される速度の単位が[m/s]で速度計の[km/h]との変換忘れによるものでした。 GNSSモジュールなんかでは[m/s]での出力が一般的なので何も考えずにそのまま値を入れているとよく分からない数値が出てきて困ります。 [ドキュメント](https://developer.sony.com/spresense/spresense-api-references-sdk/group__gnss__output__data2.html#gac46e821ae08930cc9e257642e34275a3)にも書いてありますし、ラップされたソースにも次の通りの記述があったのでちゃんと確認しなきゃダメですね... ```cpp:lib/GNSS/GNSS.cpp ... /** * @class SpNavData * @brief GNSS positioning data * * @details Store the positioning result in this object. */ class SpNavData { public: SpGnssTime time; /**< Time when this position data was updated */ unsigned char type; /**< Position type; 0:Invalid, 1:GNSS, 2:reserv, 3:user set, 4:previous */ unsigned char numSatellites; /**< Number of visible satellites */ unsigned char posFixMode; /**< FIX mode, 1:Invalid, 2:2D FIX, 3:3D FIX */ unsigned char posDataExist; /**< Is position data existed, 0:none, 1:exist */ unsigned char numSatellitesCalcPos; /**< Number of satellites to calculate the position */ unsigned short satelliteType; /**< using sv system, bit field; bit0:GPS, bit1:GLONASS */ unsigned short posSatelliteType; /**< using sv system, bit field; bit0:GPS, bit1:GLONASS */ double latitude; /**< Latitude [degree] */ double longitude; /**< Longitude [degree] */ double altitude; /**< Altitude [meter] */ float velocity; /**< Velocity [m/s] */ float direction; /**< Direction [degree] */ float pdop; /**< Position DOP [-] */ float hdop; /**< Horizontal DOP [-] */ float vdop; /**< Vertical DOP [-] */ float tdop; /**< Time DOP [-] */ SpSatellite satellite[24]; /**< satellite data array */ ... ``` ## どれくらい受信できていたのかの確認 PCとの通信記録を確認すると次のようになっていました。  動画を撮ったころの時間の衛星配置を[GNSS View](https://app.qzss.go.jp/GNSSView/gnssview.html)で確認すると、次のようになっています。  これらから、衛星の数だけではGPS1機を除いてすべての衛星からの信号を受信できていたことがわかります。 日本付近ではみちびき衛星群から出ている位置情報の測定精度を上げるための信号であるL1S信号を受信することで1mの精度で測定ができるようですが、このときはL1S信号は受信できていなかったようでした。 多くの衛星の信号を受信できていたとしても、様々な要因（受信できた信号が建物などで反射した信号だったなど）により正確な位置が導き出せないことがあるようです。 この時のログではNoFixとなっていたので、L1Sを受信できていなかったこともありそこまでの精度は出ていなかったことでしょう。 せっかくSpresenseはL1Sに対応してるので受信してほしかったですね。 ## お気持ちと言い訳 ほんとはこのコンテスト期間中にもっといろいろやるつもりで開発計画を練っていました。 コミケの原稿とか原稿関連の開発がギリギリになったりとかで全然時間が取れず、結局速度計を動かすだけになっちゃいました。 せっかくのSpresenseなので、信号とか標識の情報を認識させて表示できたらいいなと考えていました。 実は途中まで開発していたのですがうまくいかず、期限も迫っていたので今回はあきらめてまたの機会にすることにしました。 本来の開発計画のうちまだまだ最初の少ししかできていないので、今度時間があるタイミングで残りもちまちまやって行こうと思います... これ読み返すとコードが長すぎて読みにくいですよね。 elchikaさん、折り畳み表示を実装するかhtmlタグの記述を許してくれませんかねぇ～ # 参考文献 - [SpresenseのGNSS測定周期を変更することは可能ですか？](https://ja.stackoverflow.com/questions/77596/spresense%E3%81%AEgnss%E6%B8%AC%E5%AE%9A%E5%91%A8%E6%9C%9F%E3%82%92%E5%A4%89%E6%9B%B4%E3%81%99%E3%82%8B%E3%81%93%E3%81%A8%E3%81%AF%E5%8F%AF%E8%83%BD%E3%81%A7%E3%81%99%E3%81%8B) - [SpresenseのGNSS測定周期を変更](https://inoookov.hatenablog.com/entry/2021/08/06/215332) - [ArduinoとESP32（M5Cameraとか）間でI2C通信する方法](https://qiita.com/saka-guti/items/20a9000491c93214ce01) - [ESP32 で2つ目のI2Cを動かすメモ的な何か？](https://hamayan.blog.ss-blog.jp/2019-02-21-1) [^1]: [GNSS example application](https://github.com/sonydevworld/spresense-arduino-compatible/blob/master/Arduino15/packages/SPRESENSE/hardware/spresense/1.0.0/libraries/GNSS/examples/gnss/gnss.ino)