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

以前の記事で速度計を動かしました。
でも、動かしただけでは物足りない！ということで今回は実際の速度にリアルタイムに追従して表示することを目標に開発をすることにします。
イメージとしてはこんな感じです。

ここに動画が表示されます

速度計の駆動回路は別で作ったものを用いて、GNSSで速度を取得する部分はSpresenseを用います。
簡単に図にすると次通りです。

速度計を動かす
- GNSSから速度を取得する
- 通信を介して制御基板から制御する
- PlatformIOのArduino環境でコーディング

Spresense
- メインボード
- 拡張ボード
速度計
- 阪急で使われてたやつ
- 既存の制御基板

Spresenseのメインボードや拡張ボードはモニター品を提供していただきました、ありがとうございます

Spresenseから直接出力を行うのは厳しそうだったため、既存の制御基板を通して速度計を制御することにしました。
Spresenseと制御基板の間はI2Cで通信を行い、速度計は0-140km/hなので送信されたデータのうち最下位1バイトのみを採用して、その値を丸ごと速度計の指示値とすることにしました。
I2CはSpresenseをマスター、制御基板のesp32をスレーブとして、適当に実装しました。
その辺の実装をライブラリが適当にやってくれるのは気分がいいですね。

制御基板は速度計に取り付いているので、制御基板への電源供給と通信だけで速度計を動かすことができます。
簡単に運んで使用するために、電源一本だけで動くことを目標に構成を考えます。
Spresense側のI2Cは拡張ボードの右上にあるD14, D15を使います。
制御基板には12Vから5Vに電圧を落とすコンバーターが乗ってるので、12Vをここに供給してSpresenseはコンバーターが出力する5Vをもらうことにします。
自動車なんかだと12Vがシガーからとれるので簡単ですね！
（セルモーターやオルタネーターがつながっていてきれいな12Vじゃないところにつなぐのはやめておいた方がよさそうですが...）
電源や通信回りを簡単な図にすると次の通りです。

Spresense拡張ボードはレベルシフト回路が入っていて、常に電源にプルアップされているのでシビアな環境でなければI2Cのプルアップを省略できます。
わざわざ外部でプルアップするのは面倒なのでやるとしても内蔵プルアップだとは思いますが、今回はレベルシフト回路に任せて省略します。
今回はなにもしなくても動いたので検証や調整はしていませんが、レベルシフト回路のプルアップは1kなので通信速度や環境によっては外付けの抵抗などで調整したほうがいいかもしれません。

ほとんどサンプル^[1]のパクリですが、追加や修正をしたポイントだけ触れておきます。

GNSS更新周期
- ライブラリを少しいじって実装（詳しくは下で）
- 1秒では物足りないので0.25秒に
  - ベストエフォートらしいのでタイミングによっては1秒に3回になるかも
GNSS設定
- デフォルトではGPSのみ
- 日本ではみちびき（QZSS）が利用可能
  - みちびきの衛星測位サービス（GPSと同種の信号:L1C/A）を使用
  - みちびきのサブメータ級測位補強サービス（ほかの信号と組み合わせて誤差1mで測位を行うことができる信号:L1S）を使用
ログとエラーの整備
- ログの類は全部MPLog
- エラーの類は全部MPERR
サブコアへの通知
- LCD制御用のサブコアへ位置情報を送る
- LCDをつければサブコアからLCDを制御できる
  - 実験用に使ってたレガシーコード


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");
    }
...

GNSSの更新周期が1秒では物足りないので変更します。
変更自体はSpGnss::setInterval(long interval)でできますが、引数の型がlongでしかも単位が秒です。
そこで、ライブラリのコードを改変してミリ秒単位で設定できるようにします。
16行目は元はsetdata.cycle = interval * 1000;だったところを、setdata.cycle = interval;// * 1000;とコメントアウト。
ここで引数の型がfloatか1000倍が元からなければ楽だったんですがね...


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スレーブとして動かす
速度計の制御に関する部分は既存のコードを流用

コードをドーン！！


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"); // 適当に応答を返す
}

動いた！！
実際に速度計を積んで自動車で走ってみました。（15秒くらいから動きます）

ここに動画が表示されます

720pなのと手振れがひどいこと、レンズが汚れてることはごめんなさい。
とりあえずかなりそれっぽく動いたので今回は良しとします...
課題があるとすれば、更新頻度が少し低いこと室内やトンネル内では速度がわからないことでしょうか。
車の速度計との誤差は1,2km/h程度とかなり高精度でした。
途中で0になるタイミングがありますが、これはGNSSのリセットをしているためです。
リセット後はホットスタートなので比較的速やかに測定が再開されています。

ここに動画が表示されます

この動画では30-50km/h程度で走行していますが、実際の速度とは大きくかけ離れた値が表示されています。
停車するとなんとなく0に近づくので動作自体はそれっぽいですが、速度が上がるにつれ真の値との誤差が大きくなっていくことがわかります。
この現象の原因は、出力される速度の単位[m/s]と速度計の単位[km/h]の変換忘れによるものでした。
GNSSモジュールなんかでは[m/s]での出力が一般的なので何も考えずにそのまま値を入れているとよく分からない数値が出てきて困ります。
ドキュメントにも書いてありますし、ラップされたソース^[2]にも次の通りの記述があったのでちゃんと確認しなきゃダメですね...


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で確認すると、次のようになっています。

これらから、衛星の数だけではGPS1機を除いてすべての衛星からの信号を受信できていたことがわかります。
日本付近ではみちびき衛星群から出ている位置情報の測定精度を上げるための信号であるL1S信号を受信することで1mの精度で測定ができるようですが、このときはL1S信号は受信できていなかったようでした。
多くの衛星の信号を受信できていたとしても、様々な要因（受信できた信号が建物などで反射した信号だったなど）により正確な位置が導き出せないことがあるようです。
この時のログではNoFixとなっていたので、L1Sを受信できていなかったこともありそこまでの精度は出ていなかったことでしょう。
せっかくSpresenseはL1Sに対応してるので受信してほしかったですね。

ほんとはこのコンテスト期間中にもっといろいろやるつもりで開発計画を練っていました。
コミケの原稿とか原稿関連の開発がギリギリになったりとかで全然時間が取れず、結局速度計を動かすだけになっちゃいました。
結果的に限界開発になり、締め切り当日に実験動画を撮り記事にのせて投稿というかなり無理のあるスケジュールでした。
せっかくのSpresenseなので、当初は信号とか標識の情報を認識させて表示できたらいいなと考えていたんです。
実は途中まで開発していたのですがうまくいかず、期限も迫っていたので今回はあきらめてまたの機会にすることにしました。
本来の開発計画のうちまだまだ最初の少ししかできていないので、今度時間があるタイミングで残りもちまちまやって行こうと思います...

これ読み返すとコードが長すぎて読みにくいですよね。
elchikaさん、折り畳み表示を実装するかhtmlタグの記述を許してくれませんかねぇ～