コメント無し

# はじめに

# 環境 - OS：Windows 11 Home - Arduino 2.3.6 - Spresenseメインボード - Spresense拡張ボード - SpresenseマルチIMUアドオンボード - ZED-F9Pモジュール # GNSSの保存で工夫したとこ GNSSについては、F9Pと呼ばれるGNSSモジュールを使いました。F9Pから送られてくる.ubxバイナリデータをUARTで受け取ります。更新周期が1Hz程度ではありますが、1cycleのデータがそこそこのサイズなので、flush中に受信しちゃうとバッファから漏れた分が欠損します。なので[1cycleのデータサイズ以上のバッファを用意してあげる](https://elchika.com/article/e8106140-9799-4150-85df-256b7e55c008/)と良い感じになりました。 # IMUの保存で工夫したとこ IMUについては、マルチIMUアドオンを使いました。更新周期が1920Hzなので、1cycle毎にflushするのは現実的ではありません。そこで、1920cycle分のデータを格納できるバッファを２つ用意して、片方のバッファに受信データを格納している隙に、もう片方のバッファの中身をmicroSDにflushすれば良い感じでした。 # ソースコード GNSSの受信と、GNSS・IMUデータのSDへの書き込みをmainコアで担当します。IMUの受信は何となくサブコアにやらせました。 なお、コード中で参照する NuttX/Spresense SDK のヘッダは Apache License 2.0 に従います。また Spresense Arduino ライブラリ（Arduino.h/SDHCI.h/File.h/MP.h/RTC.h/GNSS.h） は LGPL-2.1に基づき配布されています。詳細は各公式配布物のライセンス表記をご確認ください。 ## main.ino ```arduino:main.ino #include "init.h" void printClock(RtcTime& rtc) { printf("%04d/%02d/%02d %02d:%02d:%02d\n", rtc.year(), rtc.month(), rtc.day(), rtc.hour(), rtc.minute(), rtc.second()); } void updateClock() { static RtcTime old; RtcTime now = RTC.getTime(); // Display only when the second is updated if (now != old) { printClock(now); old = now; } } void syncRTCFromGnssIfAvailable() { // Wait for GNSS data if (Gnss.waitUpdate(0)) { // Get the UTC time Gnss.getNavData(&nav); SpGnssTime* time = &nav.time; // Check if the acquired UTC time is accurate if (time->year >= 2000) { RtcTime now = RTC.getTime(); // Convert SpGnssTime to RtcTime RtcTime gps(time->year, time->month, time->day, time->hour, time->minute, time->sec, time->usec * 1000); #ifdef MY_TIMEZONE_IN_SECONDS // Set the time difference gps += MY_TIMEZONE_IN_SECONDS; #endif RTC.setTime(gps); } } } // ======= UBX ACK/NAK 受信 ======= bool waitForUBXAck(uint8_t cls, uint8_t id, uint32_t timeout_ms) { const uint8_t SYNC1 = 0xB5, SYNC2 = 0x62; uint32_t t0 = millis(); uint8_t state = 0; uint8_t hdr[4]; // class,id,lenL,lenH while (millis() - t0 < timeout_ms) { if (F9P_SERIAL.available()) { uint8_t b = F9P_SERIAL.read(); switch (state) { case 0: state = (b == SYNC1) ? 1 : 0; break; case 1: state = (b == SYNC2) ? 2 : 0; break; case 2: hdr[0] = b; state = 3; break; // class case 3: hdr[1] = b; state = 4; break; // id case 4: hdr[2] = b; state = 5; break; // lenL case 5: hdr[3] = b; { uint16_t len = (uint16_t)hdr[2] | ((uint16_t)hdr[3] << 8); if (len > 32) { state = 0; break; } uint8_t payload[40]; size_t need = len + 2; // +CK_A/B size_t got = 0; uint32_t t1 = millis(); while (got < need && (millis() - t1 < timeout_ms)) { if (F9P_SERIAL.available()) payload[got++] = F9P_SERIAL.read(); } bool isACK = (hdr[0] == 0x05 && hdr[1] == 0x01); bool isNAK = (hdr[0] == 0x05 && hdr[1] == 0x00); if ((isACK || isNAK) && payload[0] == cls && payload[1] == id) { return isACK; } state = 0; break; } } } } return false; } bool sendUBX(const uint8_t* msg, size_t len, uint32_t timeout_ms = 1500) { if (len < 6) return false; uint8_t cls = pgm_read_byte(msg + 2); uint8_t id = pgm_read_byte(msg + 3); for (size_t i = 0; i < len; ++i) F9P_SERIAL.write(pgm_read_byte(msg + i)); F9P_SERIAL.flush(); bool ok = waitForUBXAck(cls, id, timeout_ms); Serial.print(F("UBX 0x")); Serial.print(cls, HEX); Serial.print(' '); Serial.print(F("0x")); Serial.print(id, HEX); Serial.println(ok ? F(" -> ACK") : F(" -> NAK/Timeout")); return ok; } void setup() { //Spresenseシリアルポート設定 Serial.begin(CONSOLE_BAUDRATE); delay(100); // GNSS UART 開始（現状速度） F9P_SERIAL.begin(F9P_BAUD); delay(100); Serial.println("Serial port OK"); // F9P設定シーケンス送信 for (size_t i = 0; i < NUM_SEQ; ++i) { if (SEQ_LEN[i] == 0) continue; sendUBX(SEQ[i], SEQ_LEN[i], 2000); // 1番目(=CFG-PRT)の直後にボーレート切替 if (i == 0 && NEW_F9P_BAUD != F9P_BAUD) { delay(500); F9P_SERIAL.end(); F9P_SERIAL.begin(NEW_F9P_BAUD); F9P_BAUD = NEW_F9P_BAUD; Serial.print(F("Switched GNSS UART to ")); Serial.println(F9P_BAUD); delay(500); } delay(200); } Serial.println("F9P Setting OK"); // RTC設定 RTC.begin(); Serial.println("RTC Setting OK"); //Spresense GNSS設定 int ret = Gnss.begin(); if (ret != 0) { Serial.println("Gnss begin error!!"); } ret = Gnss.start(); if (ret != 0) { Serial.println("Gnss start error!!"); } // GNSS時刻受信完了まで待機 int ok = 0; for (int i = 0; i < 600; ++i) { if (Gnss.waitUpdate()) { SpNavData NavData; Gnss.getNavData(&NavData); SpGnssTime* time = &NavData.time; RtcTime gps(time->year, time->month, time->day, time->hour, time->minute, time->sec, time->usec * 1000); printClock(gps); if (time->year > 2000) { Serial.print("GNSS Time fix"); break; } } delay(1000); } Serial.println("Spresense GNSS Setting OK"); //SDカード設定 Serial.println("Insert SD card."); while (!SD.begin()) { ; /* wait until SD card is mounted. */ } //SDカード保存ファイル設定 imulogfile = SD.open(IMULOG_FILENAME, FILE_WRITE); if (!imulogfile) { Serial.println(F("IMU File open failed")); while (1) ; } ubxlogfile = SD.open(UBXLOG_FILENAME, FILE_WRITE); if (!ubxlogfile) { Serial.println(F("UBX File open failed")); while (1) ; } Serial.println("SD card OK."); //サブコア設定 ret = MP.begin(subcore); if (ret < 0) { printf("MP.begin(%d) error = %d\n", subcore, ret); } MP.RecvTimeout(MP_RECV_BLOCKING); //subcore準備完了待ち int8_t msgid; void* adder; MP.Recv(&msgid, &adder, subcore); Serial.println("subcore1 OK"); memset(&apacket, 0, sizeof(apacket)); //送信用構造体初期化 memset(&bpacket, 0, sizeof(bpacket)); //送信用構造体初期化 MP.Send(msgid, &adder, subcore); //subcore1にmaincore準備完了送信 apacket.status = 0; MP.Send(msgid, &apacket, subcore); bpacket.status = 0; MP.Send(msgid, &bpacket, subcore); MP.RecvTimeout(MP_RECV_POLLING); Serial.println("Subcore Setting OK"); Serial.println("-------Start-------"); } void loop() { syncRTCFromGnssIfAvailable(); if (apacket.status == 1) { Serial.println("apacket rev"); Serial.println(apacket.data_size); if (imulogfile) { Serial.println("Writing to imu.bin..."); imulogfile.write(apacket.data_buf, apacket.data_size); imulogfile.flush(); Serial.println("done."); } else { Serial.println("error opening imu.bin"); } apacket.status = 0; apacket.data_size = 0; } if (bpacket.status == 1) { Serial.println("bpacket rev"); Serial.println(bpacket.data_size); if (imulogfile) { Serial.println("Writing to imu.bin..."); imulogfile.write(bpacket.data_buf, bpacket.data_size); imulogfile.flush(); Serial.println("done."); } else { Serial.println("error opening imu.bin"); } bpacket.status = 0; bpacket.data_size = 0; } while (F9P_SERIAL.available()) { ubxlogfile.write(F9P_SERIAL.read()); F9P_readlen = F9P_readlen + 1; } static uint32_t lastFlush = 0; if (millis() - lastFlush > 1000) { ubxlogfile.flush(); Serial.print("F9P read size:"); Serial.println(F9P_readlen); F9P_readlen = 0; lastFlush = millis(); } } ``` ## init.h (main.inoの変数とか定義したヘッダファイル) ```arduino:init.h #include <Arduino.h> #include <SDHCI.h> #include <File.h> #include <MP.h> #include <RTC.h> #include <GNSS.h> #define F9P_SERIAL Serial2 #define CONSOLE_BAUDRATE 1000000 #define MY_TIMEZONE_IN_SECONDS (9 * 60 * 60) // JST const char* UBXLOG_FILENAME = "rover.ubx"; const char* IMULOG_FILENAME = "imu.bin"; uint32_t F9P_BAUD = 38400; // F9Pの初期のUART1ボーレート #define BUF_SIZE 65536 uint32_t F9P_readlen = 0; SDClass SD; File imulogfile; File ubxlogfile; SpGnss Gnss; SpNavData nav; int subcore = 1; // ======= u-center の UBX バイナリ ======= // --- 例：CFG-PRT UART1 const uint8_t UBX_CFG_PRT_UART1[] PROGMEM = { 0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, // portID = 1 (UART1) 0xD0, 0x08, 0x00, 0x00, // mode = 0x000008D0 (8N1) 0x00, 0x84, 0x03, 0x00, // baudRate = 0x00038400 (= 230400) 0x01, 0x00, // inProtoMask = 0x0001 (UBX in) 0x01, 0x00, // outProtoMask = 0x0001 (UBX out only) 0x00, 0x00, // flags 0x00, 0x00, // reserved2 0x7C, 0xAC // CK_A, CK_B }; // 変更後に合わせる新ボーレート static uint32_t NEW_F9P_BAUD = 230400; const uint8_t UBX_CFG_PRT_UART2_115200_IN_RTCM_OUT_UBX[] PROGMEM = { 0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x02, 0x00, 0x00, 0x00, // portID=2 (UART2) 0xD0, 0x08, 0x00, 0x00, // mode=8N1 0x00, 0xC2, 0x01, 0x00, // baud=115200 0x20, 0x00, // inProtoMask = RTCM3 0x01, 0x00, // outProtoMask = UBX 0x00, 0x00, 0x00, 0x00, // flags/reserved 0xD8, 0x4E // CK_A, CK_B }; //ローバー設定 const uint8_t UBX_CFG_NAV5_PORTABLE_3D[] PROGMEM = { 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, // mask = 0xFFFF 0x00, // dynModel = 0 (Portable) 0x03, // fixMode = 3 (2D/3D Auto) 0x00, 0x00, 0x00, 0x00, // fixedAlt 0x10, 0x27, 0x00, 0x00, // fixedAltVar = 10000 0x0F, // minElev = 15 deg 0x00, // drLimit = 0 0xFA, 0x00, // pDOP = 25.0 0xFA, 0x00, // tDOP = 25.0 0x64, 0x00, // pAcc = 100 m 0x5E, 0x01, // tAcc = 350 m 0x00, // staticHoldThresh = 0 0x3C, // dgnssTimeout = 60 s 0x00, // cnoThreshNumSVs = 0 0x00, // cnoThresh = 0 0x00, 0x00, // staticHoldMaxDist = 0 0x00, // utcStandard = Auto 0x00, 0x00, 0x00, 0x00, 0x00, // reserved 0x88, 0x2C // CK_A, CK_B }; const uint8_t UBX_CFG_TMODE3_DISABLED[] PROGMEM = { 0xB5, 0x62, 0x06, 0x71, 0x28, 0x00, 0x00, 0x00, // version=0, reserved1 0x00, 0x00, // flags=0x0000 → TMODE3 Disabled 0x00, 0x00, 0x00, 0x00, // ecefX 0x00, 0x00, 0x00, 0x00, // ecefY 0x00, 0x00, 0x00, 0x00, // ecefZ 0x00, 0x00, 0x00, // ecefXHP/YHP/ZHP 0x00, // reserved2 0x00, 0x00, 0x00, 0x00, // fixedPosAcc 0x00, 0x00, 0x00, 0x00, // svinMinDur 0x00, 0x00, 0x00, 0x00, // svinAccLimit 0x00, 0x00, 0x00, 0x00, // reserved3[0..3] 0x00, 0x00, 0x00, 0x00, // reserved3[4..7] 0x9F, 0x93 // CK_A, CK_B }; // --- 出力メッセージ設定 --- const uint8_t UBX_CFG_MSG_BLOCK[] PROGMEM = { 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x07, // NAV-PVT 0x00, 0x01, // I2C=0, UART1=1 0x00, 0x01, // UART2=0, USB=1 0x00, 0x00, // SPI=0, reserved=0 0x19, 0xE4, // CK_A, CK_B 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x15, // RXM-RAWX 0x00, 0x01, // I2C=0, UART1=1 0x00, 0x01, // UART2=0, USB=1 0x00, 0x00, // SPI=0, reserved=0 0x28, 0x4E, // CK_A, CK_B 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x13, // RXM-SFRBX 0x00, 0x01, // I2C=0, UART1=1 0x00, 0x01, // UART2=0, USB=1 0x00, 0x00, // SPI=0, reserved=0 0x26, 0x40, // CK_A, CK_B 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF5, 0x05, // RTCM3 class, msg 1005 0x00, 0x00, // I2C=0, UART1=0 0x01, 0x00, // UART2=1, USB=0 0x00, 0x00, // SPI=0, reserved=0 0x0A, 0x72, // CK_A, CK_B 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF5, 0x4D, // RTCM3 class, type 1077 (GPS MSM7) 0x00, 0x00, // I2C=0, UART1=0 0x01, 0x00, // UART2=1, USB=0 ← UART2へ毎秒出力 0x00, 0x00, // SPI=0, reserved=0 0x52, 0x6A, // CK_A, CK_B, 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF5, 0x57, // RTCM3 class, type 1087 (GLONASS MSM7) 0x00, 0x00, // I2C=0, UART1=0 0x01, 0x00, // UART2=1, USB=0 ← UART2へ毎秒出力 0x00, 0x00, // SPI=0, reserved=0 0x5C, 0xB0, // CK_A, CK_B 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF5, 0x61, // RTCM3 class, type 1097 (Galileo MSM7) 0x00, 0x00, // I2C=0, UART1=0 0x01, 0x00, // UART2=1, USB=0 ← UART2へ毎秒出力 0x00, 0x00, // SPI=0, reserved=0 0x66, 0xF6, // CK_A, CK_B 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF5, 0x7F, // RTCM3 class, type 1127 (BeiDou MSM7) 0x00, 0x00, // I2C=0, UART1=0 0x01, 0x00, // UART2=1, USB=0 ← UART2へ毎秒出力 0x00, 0x00, // SPI=0, reserved=0 0x84, 0xC8, // CK_A, CK_B 0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF5, 0xE6, // RTCM3 class, type 1230 (GLONASS code-phase biases) 0x00, 0x00, // I2C=0, UART1=0 0x01, 0x00, // UART2=1, USB=0 ← UART2へ毎秒出力 0x00, 0x00, // SPI=0, reserved=0 0xEB, 0x99 // CK_A, CK_B }; // --- 保存 (CFG-CFG) --- const uint8_t UBX_CFG_CFG_SAVE[] PROGMEM = { 0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, // clearMask = 0 0xFF, 0xFF, 0x00, 0x00, // saveMask = 0x0000FFFF (全カテゴリ保存) 0x00, 0x00, 0x00, 0x00, // loadMask = 0 0x03, // deviceMask = 0x03 (BBR + Flash) 0x1D, 0xAB // CK_A, CK_B }; // 送信シーケンス const uint8_t* const SEQ[] = { UBX_CFG_PRT_UART1, UBX_CFG_PRT_UART2_115200_IN_RTCM_OUT_UBX, UBX_CFG_NAV5_PORTABLE_3D, UBX_CFG_TMODE3_DISABLED, UBX_CFG_MSG_BLOCK, UBX_CFG_CFG_SAVE, }; const size_t SEQ_LEN[] = { sizeof(UBX_CFG_PRT_UART1), sizeof(UBX_CFG_PRT_UART2_115200_IN_RTCM_OUT_UBX), sizeof(UBX_CFG_NAV5_PORTABLE_3D), sizeof(UBX_CFG_TMODE3_DISABLED), sizeof(UBX_CFG_MSG_BLOCK), sizeof(UBX_CFG_CFG_SAVE), }; const size_t NUM_SEQ = sizeof(SEQ) / sizeof(SEQ[0]); struct APacket { volatile int status = 0; //data_bufに最新データが格納されている時は1、maincoreでSD保存が完了した時は0 uint32_t data_size = 0; //データサイズ uint8_t data_buf[65536]; }; APacket apacket; struct BPacket { volatile int status = 0; //data_bufに最新データが格納されている時は1、maincoreでSD保存が完了した時は0 uint32_t data_size = 0; //データサイズ uint8_t data_buf[65536]; }; BPacket bpacket; ``` ## sub1.ino ```arduino:sub1.ino #include "init.h" static int start_sensing(int fd, int rate, int adrange, int gdrange, int nfifos) { cxd5602pwbimu_range_t range; ioctl(fd, SNIOC_SSAMPRATE, rate); range.accel = adrange; range.gyro = gdrange; ioctl(fd, SNIOC_SDRANGE, (unsigned long)(uintptr_t)&range); ioctl(fd, SNIOC_SFIFOTHRESH, nfifos); ioctl(fd, SNIOC_ENABLE, 1); return 0; } static int drop_50msdata(int fd, int samprate) { int cnt = samprate / 20; /* data size of 50ms */ cnt = ((cnt + MAX_NFIFO - 1) / MAX_NFIFO) * MAX_NFIFO; if (cnt == 0) cnt = MAX_NFIFO; while (cnt) { read(fd, g_data, sizeof(g_data[0]) * MAX_NFIFO); cnt -= MAX_NFIFO; } return 0; } static int dump_data(int fd) { int c; int ret; int buffer_sw = 0; //0→a 1→b while (1) { ret = read(fd, g_data, sizeof(g_data[0]) * MAX_NFIFO); if (ret == sizeof(g_data[0]) * MAX_NFIFO) { RtcTime now = RTC.getTime(); uint32_t now_unixtime = now.unixtime(); long now_nsec = now.nsec(); if (buffer_sw == 0) { memcpy(&apacket->data_buf[addr_buffa], &header_data, sizeof(header_data)); addr_buffa = addr_buffa + sizeof(header_data); memcpy(&apacket->data_buf[addr_buffa], &now_unixtime, sizeof(now_unixtime)); addr_buffa = addr_buffa + sizeof(now_unixtime); memcpy(&apacket->data_buf[addr_buffa], &now_nsec, sizeof(now_nsec)); addr_buffa = addr_buffa + sizeof(now_nsec); memcpy(&apacket->data_buf[addr_buffa], reinterpret_cast<const uint8_t *>(&g_data), sizeof(g_data)); addr_buffa = addr_buffa + sizeof(g_data[0]) * MAX_NFIFO; memcpy(&apacket->data_buf[addr_buffa], &footer_data, sizeof(footer_data)); addr_buffa = addr_buffa + sizeof(footer_data); if (addr_buffa > sizeof(apacket->data_buf) - sizeof(g_data[0]) * MAX_NFIFO - ADD_DATASIZE) { apacket->data_size = addr_buffa; apacket->status = 1; addr_buffa = 0; buffer_sw = 1; } } else { memcpy(&bpacket->data_buf[addr_buffb], &header_data, sizeof(header_data)); addr_buffb = addr_buffb + sizeof(header_data); memcpy(&bpacket->data_buf[addr_buffb], &now_unixtime, sizeof(now_unixtime)); addr_buffb = addr_buffb + sizeof(now_unixtime); memcpy(&bpacket->data_buf[addr_buffb], &now_nsec, sizeof(now_nsec)); addr_buffb = addr_buffb + sizeof(now_nsec); memcpy(&bpacket->data_buf[addr_buffb], reinterpret_cast<const uint8_t *>(&g_data), sizeof(g_data)); addr_buffb = addr_buffb + sizeof(g_data[0]) * MAX_NFIFO; memcpy(&bpacket->data_buf[addr_buffb], &footer_data, sizeof(footer_data)); addr_buffb = addr_buffb + sizeof(footer_data); if (addr_buffb > sizeof(bpacket->data_buf) - sizeof(g_data[0]) * MAX_NFIFO - ADD_DATASIZE) { bpacket->data_size = addr_buffb; bpacket->status = 1; addr_buffb = 0; buffer_sw = 0; } } } } } void setup() { Serial.begin(CONSOLE_BAUDRATE); MP.begin(); MP.RecvTimeout(MP_RECV_BLOCKING); RTC.begin(); int8_t msgid = 0; //メッセージID void *adder; MP.Send(msgid, &adder); //準備完了をメインコアに通知 MP.Recv(&msgid, &adder); //メインコアの準備完了通知を受信 MP.Recv(&msgid, &apacket); MP.Recv(&msgid, &bpacket); MP.RecvTimeout(MP_RECV_POLLING); //コア間通信ブロッキングからポーリングへ int devfd; board_cxd5602pwbimu_initialize(5); devfd = open(CXD5602PWBIMU_DEVPATH, O_RDONLY); start_sensing(devfd, S_FQ, 16, 2000, MAX_NFIFO); drop_50msdata(devfd, S_FQ); dump_data(devfd); } void loop() { } ``` ## init.h (sub1.inoの変数とか定義したヘッダファイル) ```arduino:init.ino #include "stdint.h" #include <MP.h> #include <sys/ioctl.h> #include <fcntl.h> #include <nuttx/sensors/cxd5602pwbimu.h> #include <arch/board/cxd56_cxd5602pwbimu.h> #include <RTC.h> #define CXD5602PWBIMU_DEVPATH "/dev/imu0" #define MAX_NFIFO (4) #define CONSOLE_BAUDRATE 1000000 #define S_FQ 1920 #define ADD_DATASIZE 12 uint8_t header_data[2] = {0xa5,0x5a}; uint8_t footer_data[2] = {0x0d,0x0a}; static cxd5602pwbimu_data_t g_data[MAX_NFIFO]; struct APacket { volatile int status = 0; //data_bufに最新データが格納されている時は1、maincoreでSD保存が完了した時は0 uint32_t data_size = 0; //データサイズ uint8_t data_buf[65536]; }; APacket *apacket; uint32_t addr_buffa = 0; struct BPacket { volatile int status = 0; //data_bufに最新データが格納されている時は1、maincoreでSD保存が完了した時は0 uint32_t data_size = 0; //データサイズ uint8_t data_buf[65536]; }; BPacket *bpacket; uint32_t addr_buffb = 0; ```