Last active
March 1, 2025 02:23
-
-
Save glyzinie/a4bebfa93e8cd1f542cff9484f8be36f to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #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