ESP32でGPSラップタイマーを自作してみる

 知り合いがGPSラップタイマーが欲しいけど高いよーとか言っているのでこの前購入したGPSモジュールで試しに作ってみることに。

とりあえずESP32でGPSのMNEAセンテンスを読んで、指定している座標Aと座標Bの間を通ったらカウントする感じ。これならタイム測定ポイントを正確に指定できるかなと。最終的にはWeb設定画面を表示して設定変更したり、内部メモリで選択できるようにしたいな。

まずはちゃんと座標間を通ったときにカウントできるかのテスト。最初に通ったらカウントを開始して、2回目通ったらLAPタイムをSerial.printするだけのテスト。M5Atom Liteで動作確認してみた。

#define GPS_RX 32
#define GPS_TX 26

char nmeaLine[128];
int idx = 0;
double LAT0 = 38.14;
double LON0 = 140.77;

HardwareSerial GPS(2);

struct Vec2 {
  double x;
  double y;
  uint64_t time_ms;  // 経過ms
};

struct Line {
  double x1, y1, x2, y2;
};

Vec2 prevPos;
bool firstFix = true;
bool firstLineCross = true;
uint64_t lastLapTime = 0;

Vec2 latlonToXY(double lat, double lon, uint64_t t) {
  Vec2 p;
  p.x = (lon - LON0) * cos(LAT0 * DEG_TO_RAD) * 111320.0;
  p.y = (lat - LAT0) * 110540.0;
  p.time_ms = t;
  return p;
}

double cross(double ax, double ay, double bx, double by) {
  return ax * by - ay * bx;
}

double signedDistance(const Vec2& p, const Line& L) {
  double lx = L.x2 - L.x1;
  double ly = L.y2 - L.y1;
  double px = p.x - L.x1;
  double py = p.y - L.y1;
  return cross(lx, ly, px, py);
}

bool checkLineCross(const Vec2& prev, const Vec2& now, const Line& line, uint64_t& crossTime) {
  double d1 = signedDistance(prev, line);
  double d2 = signedDistance(now, line);
  if (d1 * d2 >= 0) return false;
  // 交点の補間
  double ratio = fabs(d1) / (fabs(d1) + fabs(d2));
  double cx = prev.x + (now.x - prev.x) * ratio;
  double cy = prev.y + (now.y - prev.y) * ratio;
  // 線分範囲内判定
  double minX = fmin(line.x1, line.x2);
  double maxX = fmax(line.x1, line.x2);
  double minY = fmin(line.y1, line.y2);
  double maxY = fmax(line.y1, line.y2);
  if (cx < minX || cx > maxX || cy < minY || cy > maxY)
    return false;  // 線分の延長上なら無視
  crossTime = prev.time_ms + (uint64_t)((now.time_ms - prev.time_ms) * ratio);
  return true;
}
Line startLine;
void setupStartLine() {
  Vec2 A = latlonToXY(38.142650747977115, 140.77813371163867, 0);
  Vec2 B = latlonToXY(38.14193773632026, 140.77804079991844, 0);
  startLine = { A.x, A.y, B.x, B.y };
}

double nmeaToDeg(const char* val, const char* dir) {
  double raw = atof(val);
  int deg = (int)(raw / 100);
  double min = raw - deg * 100;
  double result = deg + min / 60.0;

  if (dir[0] == 'S' || dir[0] == 'W')
    result *= -1;
  return result;
}

void parseRMC(char* line) {
  char* token;
  int field = 0;
  char utc[16] = { 0 };
  char status[2] = { 0 };
  char lat[16] = { 0 }, lon[16] = { 0 };
  char ns[2] = { 0 }, ew[2] = { 0 };
  char date[8] = { 0 };
  token = strtok(line, ",");
  while (token) {
    field++;
    if (field == 2) strcpy(utc, token);
    if (field == 3) strcpy(status, token);
    if (field == 4) strcpy(lat, token);
    if (field == 5) strcpy(ns, token);
    if (field == 6) strcpy(lon, token);
    if (field == 7) strcpy(ew, token);
    if (field == 10) strcpy(date, token);
    token = strtok(NULL, ",");
  }
  if (status[0] != 'A') return;
  double latitude = nmeaToDeg(lat, ns);
  double longitude = nmeaToDeg(lon, ew);
  uint64_t gpsTime = esp_timer_get_time() / 1000;  // μs → ms

  if (firstFix) {
    LAT0 = latitude;
    LON0 = longitude;
    setupStartLine();
    prevPos = latlonToXY(latitude, longitude, gpsTime);
    firstFix = false;
    Serial.println("Fix complete, waiting for START line...");
    return;
  }
  Vec2 pos = latlonToXY(latitude, longitude, gpsTime);
  // ラップ判定
  uint64_t crossTime;
  if (checkLineCross(prevPos, pos, startLine, crossTime)) {
    if (firstLineCross) {
      lastLapTime = crossTime;
      firstLineCross = false;
      Serial.println("START!");
    } else if (crossTime - lastLapTime > 8000) {
      uint64_t lapMs = crossTime - lastLapTime;
      uint32_t minutes = lapMs / 60000;
      uint32_t seconds = (lapMs % 60000) / 1000;
      uint32_t millisec = lapMs % 1000;
      lastLapTime = crossTime;
      Serial.printf("LAP! %02u:%02u.%03u\n", minutes, seconds, millisec);
    }
  }
  prevPos = pos;
}

void readGPS() {
  while (GPS.available()) {
    char c = GPS.read();
    if (c == '\n') {
      nmeaLine[idx] = 0;
      if (strstr(nmeaLine, "GNRMC") || strstr(nmeaLine, "GPRMC")) {
        parseRMC(nmeaLine);
      }
      idx = 0;
    } else if (idx < 127)
      nmeaLine[idx++] = c;
  }
}
void setup() {
  Serial.begin(115200);
  GPS.begin(115200, SERIAL_8N1, GPS_RX, GPS_TX);
  setupStartLine();  // 初期値
  Serial.println("GPS LapTimer");
}
void loop() {
  readGPS();
}

実際にサーキットを走ったりして確認するのは大変なので、まずは先日作ったシリアルロガーで、近所を歩いて一周してきたログをPythonでシリアルポートに流して動作確認をしてみたが、とりあえずは動いてそう。10Hzで取ったログを10倍速とかで流してもちゃんと反応してくれる。(Baudrate的に本当に10倍速になってるかはわからないけど…)
ちなみにMNEAセンテンスは絞ってる。

あとはディスプレイに表示できるようにしないと。屋外でも視認性が良さそうなディスプレイを探さないとな。あとはセクタータイム測定したり、GPSのデータをそのままロギングできたら便利そう。

ESP32でシリアルデータをロギングしてみた。

 先日ATGM336Hの動作確認ができたので、実際にGPSから出ているデータをロギングしてみることに。

今回はESP32とWebSocketを使って、ブラウザ上にデータを表示してそれをログデータとして保存してみようと思う。外でノートパソコンを持ってウロウロするのもアレだし…

#include <WiFi.h>
#include <WebServer.h>
#include <WebSocketsServer.h>

const char* ssid = "ESP32_LOG";
const char* password = "12345678";

WebServer server(80);
WebSocketsServer webSocket = WebSocketsServer(8080);

#define RXD2 32
#define TXD2 26 

const char MAIN_page[] PROGMEM = R"rawliteral(
<!DOCTYPE html>
<html>
<head>
<meta charset="UTF-8">
<title>ESP32 Serial2 Logger</title>
<style>
body { font-family: monospace; margin:20px; }
textarea {
  width:100%;
  height:400px;
}
button {
  font-size:18px;
  padding:8px 20px;
}
</style>
</head>
<body>
<h2>ESP32 UART2 Monitor</h2>
<textarea id="log"></textarea><br><br>
<button onclick="saveLog()">保存してダウンロード</button>
<script>
let ws;
function connectWS(){
  ws = new WebSocket("ws://" + location.hostname + ":8080/");
  ws.onmessage = function(event) {
    const ta = document.getElementById("log");
    ta.value += event.data;
    ta.scrollTop = ta.scrollHeight;
  };
  ws.onclose = function(){
    // 自動再接続
    setTimeout(connectWS, 2000);
  };
}
connectWS();
function saveLog() {
  const text = document.getElementById("log").value;
  const blob = new Blob([text], {type: "text/plain"});
  const url = URL.createObjectURL(blob);
  const a = document.createElement("a");
  a.href = url;
  a.download = "serial2_log.txt";
  a.click();
  URL.revokeObjectURL(url);
}
</script>
</body>
</html>
)rawliteral";

void setup()
{
  Serial.begin(115200);
  Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);
  WiFi.softAP(ssid, password);
  Serial.println("AP Started");
  Serial.println(WiFi.softAPIP());
  server.on("/", []() {
    server.send_P(200, "text/html", MAIN_page);
  });
  server.begin();
  webSocket.begin();
}

void loop()
{
  server.handleClient();
  webSocket.loop();

  static String line = "";

  while (Serial2.available())
  {
    char c = Serial2.read();
    //Serial.write(c);
    line += c;
    if (c == '\n')
    {
      webSocket.broadcastTXT(line);
      line = "";
    }
  }
}

こんな感じでとりあえずブラウザのtextareaに淡々と表示し続けて最後にsaveボタンを押す簡単なやつにしてみた。

10Hz設定だと流石に流れるのが早すぎるのか表示が追いついていない気がする。WiFi切ってもしばらく流れてるし。とりあえずiPhoneで保存したやつをPCに送って拡張子を.nmeaに変更。GSXSeeで開くとちゃんと軌跡が表示できた。

ATGM336HなGPSモジュールを買ってみた

 アリエクで安いGPSモジュールがあったので購入してみた。ATGM336HっていうシリーズのGPS。580円だった。AT6558が使われてるモジュールらしい。

とりあえずアンテナは小さいやつがついているのにしてみたけども、感度はどうなんだろう？

裏面にはホットスタート用のバッテリとEEPROM(24C32A)がついている。以前UBX-G7020のGPSモジュールを購入したときはEEPROMがついてなくて設定が保存できなかったので設定が保存できるのは便利そう。あと3.3VレギュレータのRT9193-33GBが付いているのでこれならVCCに5V入れても大丈夫そう。ただしTXとRXは3.3Vレベルなので注意。
アンテナのコネクタとバッテリが近いのでアンテナのコネクタがバッテリーに触らないように注意かも。テープでも貼っておくか…

とりあえず動作確認をしてみた。適当なシリアル変換アダプタに接続して、GnssToolKit3で動作確認ができる。室内だと窓際で1個補足できたぐらいでFIXすることはできなかった。

$GPTXT,01,01,02,MA=CASIC*27
$GPTXT,01,01,02,IC=AT6558F-5N-32-1C580900*06
$GPTXT,01,01,02,SW=URANUS5,V5.3.0.0*1D
$GPTXT,01,01,02,TB=2020-03-26,13:25:12*4B
$GPTXT,01,01,02,MO=GB*77

起動時にはこんな感じでバージョン情報とかが出る模様。

デフォルトのBaudrateは9600なんだけど、10Hzとかで動かしたら足りなくなるかもしれないということでBaudrateを変更してみた。

$PCAS01,5*19

をArduino IDEとかのシリアルコンソールからCR+CF付きで送信すると即座にBaudrateが115200に変更できた。もとに戻すには

$PCAS01,1*1D

を送ると9600に戻る。電源を切っても保持されるっぽいので設定時に一回送ればOK。

次にアップデートレートの変更だけども、10Hzに変更するには

$PCAS02,100*1E

のようにコマンドを送れば10Hz(100ms周期)でデータが送られてくる。1Hzに戻すには

$PCAS02,1000*2E

とすればOK。他にも色々変更できるっぽいけど、設定は先程のGnssToolKit3を使ったほうが便利かもしれない。GUIで色々いじれる。View→Configurationで設定画面が開ける。

バイク用にバンク角もログれるGPSロガーがほしいという知り合いがいたのでとりあえずNMEAのGPSなのであとは独自のセンテンスを追加してやれば一緒にログれそうな気がする。

MNEAセンテンスをロギング用にGGA + RMC + ZDAだけに絞るには

$PCAS03,1,0,0,0,1,0,1,0,0,0,,,0,0*03

このコマンドでおｋ。もとに戻す場合は

$PCAS03,1,1,1,1,1,1,1,1,0,1,,,1,0*02

こんな感じでもとに戻った気がする。そういえばSaveコマンドとか送らなくても全部保存されているようで、再起動してもそのままだった。

あとはツール側でそれを表示できるかどうか？とりあえずはGPXSeeっていうフリーソフトで試してみようかな。
まずは適当にログれるようなやつをM5 Atomあたりで作ってみようかな。