Skip to content

Instantly share code, notes, and snippets.

@glyzinie
Last active March 1, 2025 02:23
Show Gist options
  • Save glyzinie/a4bebfa93e8cd1f542cff9484f8be36f to your computer and use it in GitHub Desktop.
Save glyzinie/a4bebfa93e8cd1f542cff9484f8be36f to your computer and use it in GitHub Desktop.
#include <M5Unified.h>
// NMEA文受信用バッファサイズ
#define NMEA_BUFFER_SIZE 128
// 定数化:ヘッダー部分のトークン数
#define GSV_HEADER_SKIP 3 // GSV文:$GPGSV,総メッセージ数,メッセージ番号,視認衛星数 のスキップ
#define GSA_HEADER_SKIP 2 // GSA文:$GPGSA,モード,固定状態 のスキップ
#define GSA_USED_COUNT 12 // GSA文:使用中衛星番号の項目数
// GPSシリアル(UART2:RX=1, TX=2, 9600bps)
HardwareSerial gpsSerial(2);
// M5Canvas は M5Unified.h 経由で M5GFX.h により利用可能
M5Canvas canvas(&M5.Display);
// 衛星情報構造体(GSVパース用)
struct SatData {
int prn;
int elevation; // 0~90度
int azimuth; // 0~359度
int snr; // 信号強度(dB)
unsigned long tm; // 最終更新時刻(秒)
};
#define MAX_SATELLITES 20
SatData satellites[MAX_SATELLITES];
int satCount = 0;
// 使用中衛星(GSAパース用)
#define MAX_USED 20
int usedSatellites[MAX_USED];
int usedCount = 0;
unsigned long usedUpdateTime = 0;
// VTGパース用構造体
struct VTGData {
double trueCourse;
double magneticCourse;
double speedKnots;
double speedKmph;
};
VTGData vtgData = {0, 0, 0, 0};
// 描画用中心座標とグリッド半径
int centerX, centerY, gridRadius;
// 共有データ保護用ミューテックス
SemaphoreHandle_t gpsDataMutex;
// ----------------------------------------------------
// チェックサム検証関数
// ----------------------------------------------------
bool verifyChecksum(const char *sentence) {
if (sentence[0] != '$') return false;
const char *p = sentence + 1;
uint8_t cs = 0;
while (*p && *p != '*') {
cs ^= *p;
p++;
}
if (*p != '*') return false;
int provided;
if (sscanf(p + 1, "%2x", &provided) != 1) return false;
return (cs == provided);
}
// ----------------------------------------------------
// NMEA文パース
// ----------------------------------------------------
void parseGSV(char *line) {
char *saveptr;
char *token = strtok_r(line, ",*", &saveptr);
for (int i = 0; i < GSV_HEADER_SKIP; i++) {
token = strtok_r(NULL, ",*", &saveptr);
if (!token) return;
}
while ((token = strtok_r(NULL, ",*", &saveptr)) != NULL) {
int prn = atoi(token);
token = strtok_r(NULL, ",*", &saveptr);
if (!token) break;
int elev = atoi(token);
token = strtok_r(NULL, ",*", &saveptr);
if (!token) break;
int azim = atoi(token);
token = strtok_r(NULL, ",*", &saveptr);
if (!token) break;
int snr = atoi(token);
bool found = false;
for (int i = 0; i < satCount; i++) {
if (satellites[i].prn == prn) {
satellites[i].elevation = elev;
satellites[i].azimuth = azim;
satellites[i].snr = snr;
satellites[i].tm = millis() / 1000;
found = true;
break;
}
}
if (!found && satCount < MAX_SATELLITES) {
satellites[satCount].prn = prn;
satellites[satCount].elevation = elev;
satellites[satCount].azimuth = azim;
satellites[satCount].snr = snr;
satellites[satCount].tm = millis() / 1000;
satCount++;
}
}
}
void parseGSA(char *line) {
char *saveptr;
char *token = strtok_r(line, ",*", &saveptr);
for (int i = 0; i < GSA_HEADER_SKIP; i++) {
token = strtok_r(NULL, ",*", &saveptr);
if (!token) return;
}
usedCount = 0;
for (int i = 0; i < GSA_USED_COUNT; i++) {
token = strtok_r(NULL, ",*", &saveptr);
if (!token) break;
if (token[0] != '\0') {
if (usedCount < MAX_USED)
usedSatellites[usedCount++] = atoi(token);
}
}
usedUpdateTime = millis() / 1000;
}
void parseVTG(char *line) {
char *saveptr;
char *token = strtok_r(line, ",*", &saveptr);
token = strtok_r(NULL, ",*", &saveptr); // 真方位
if (!token) return;
double trueCourse = atof(token);
token = strtok_r(NULL, ",*", &saveptr); // "T"
token = strtok_r(NULL, ",*", &saveptr); // 磁方位
if (!token) return;
double magneticCourse = atof(token);
token = strtok_r(NULL, ",*", &saveptr); // "M"
token = strtok_r(NULL, ",*", &saveptr); // ノット速度
if (!token) return;
double speedKnots = atof(token);
token = strtok_r(NULL, ",*", &saveptr); // "N"
token = strtok_r(NULL, ",*", &saveptr); // km/h 速度
if (!token) return;
double speedKmph = atof(token);
vtgData.trueCourse = trueCourse;
vtgData.magneticCourse = magneticCourse;
vtgData.speedKnots = speedKnots;
vtgData.speedKmph = speedKmph;
}
// ----------------------------------------------------
// GPS受信処理
// ----------------------------------------------------
void processGPS() {
char lineBuffer[NMEA_BUFFER_SIZE];
size_t len = gpsSerial.readBytesUntil('\n', lineBuffer, NMEA_BUFFER_SIZE - 1);
if (len > 0) {
lineBuffer[len] = '\0';
if (!verifyChecksum(lineBuffer)) return;
if (lineBuffer[0] != '$') return;
// 文種判定を固定位置(lineBuffer+3, 3文字)で高速化
if (memcmp(lineBuffer + 3, "GSV", 3) == 0) {
parseGSV(lineBuffer);
} else if (memcmp(lineBuffer + 3, "GSA", 3) == 0) {
parseGSA(lineBuffer);
} else if (memcmp(lineBuffer + 3, "VTG", 3) == 0) {
parseVTG(lineBuffer);
}
}
}
// ----------------------------------------------------
// 描画処理
// ----------------------------------------------------
void drawGrid() {
canvas.fillScreen(BLACK);
for (int r = gridRadius / 3; r <= gridRadius; r += gridRadius / 3) {
canvas.drawCircle(centerX, centerY, r, DARKGREY);
}
for (int angle = 0; angle < 360; angle += 45) {
float rad = radians(angle);
int x = centerX + cos(rad) * gridRadius;
int y = centerY + sin(rad) * gridRadius;
canvas.drawLine(centerX, centerY, x, y, DARKGREY);
}
canvas.setTextSize(2);
canvas.drawString("N", centerX - 8, centerY - gridRadius - 10);
canvas.drawString("S", centerX - 8, centerY + gridRadius + 2);
canvas.drawString("E", centerX + gridRadius + 2, centerY - 8);
canvas.drawString("W", centerX - gridRadius - 12, centerY - 8);
canvas.setTextSize(1);
}
void drawSatellites() {
for (int i = 0; i < satCount; i++) {
SatData sat = satellites[i];
int r = map(90 - sat.elevation, 0, 90, 0, gridRadius);
float rad = radians(sat.azimuth);
int x = centerX + r * sin(rad);
int y = centerY - r * cos(rad);
uint16_t color = BLUE;
for (int j = 0; j < usedCount; j++) {
if (sat.prn == usedSatellites[j]) {
color = GREEN;
break;
}
}
canvas.fillCircle(x, y, 4, color);
canvas.drawCircle(x, y, 4, WHITE);
canvas.drawString(String(sat.prn), x + 8, y - 7);
}
}
// ----------------------------------------------------
// FreeRTOSタスク
// ----------------------------------------------------
void gpsTask(void *parameter) {
gpsSerial.begin(9600, SERIAL_8N1, 1, 2);
while (true) {
xSemaphoreTake(gpsDataMutex, portMAX_DELAY);
processGPS();
xSemaphoreGive(gpsDataMutex);
vTaskDelay(10 / portTICK_PERIOD_MS);
}
}
void displayTask(void *parameter) {
M5.Display.setRotation(1);
centerX = M5.Display.width() / 2;
centerY = M5.Display.height() / 2;
gridRadius = min(M5.Display.width(), M5.Display.height()) / 2 - 16;
canvas.createSprite(M5.Display.width(), M5.Display.height());
canvas.fillScreen(BLACK);
canvas.pushSprite(0, 0);
while (true) {
vTaskDelay(1000 / portTICK_PERIOD_MS);
xSemaphoreTake(gpsDataMutex, portMAX_DELAY);
canvas.fillScreen(BLACK);
drawGrid();
drawSatellites();
xSemaphoreGive(gpsDataMutex);
canvas.pushSprite(0, 0);
}
}
void setup() {
M5.begin();
gpsDataMutex = xSemaphoreCreateMutex();
xTaskCreate(gpsTask, "GPSTask", 4096, NULL, 1, NULL);
xTaskCreate(displayTask, "DisplayTask", 4096, NULL, 1, NULL);
}
void loop() {
M5.update();
vTaskDelay(1000 / portTICK_PERIOD_MS);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment