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のデータをそのままロギングできたら便利そう。