知り合いがGPSラップタイマーが欲しいけど高いよーとか言っているのでこの前購入したGPSモジュールで試しに作ってみることに。ATGM336Hのスペック上10Hzまで行けるらしいし。
とりあえず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のデータをそのままロギングできたら便利そう。
