diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 183c4d1536..f53acbc8da 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -1016,39 +1016,57 @@ uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc } /* - temporary method to use file as GPS data + read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED */ #if AP_SIM_GPS_FILE_ENABLED void GPS::update_file() { - static int fd = -1; - static int fd2 = -1; - int temp_fd; - if (instance == 0) { - if (fd == -1) { - fd = open("/tmp/gps.dat", O_RDONLY|O_CLOEXEC); - } - temp_fd = fd; - } else { - if (fd2 == -1) { - fd2 = open("/tmp/gps2.dat", O_RDONLY|O_CLOEXEC); - } - temp_fd = fd2; - } - - if (temp_fd == -1) { + static int fd[2] = {-1,-1}; + static uint32_t base_time[2]; + const uint16_t lognum = 9; + if (instance > 1) { return; } - char buf[200]; - ssize_t ret = ::read(temp_fd, buf, sizeof(buf)); - if (ret > 0) { - ::printf("wrote gps %u bytes\n", (unsigned)ret); - write_to_autopilot((const char *)buf, ret); + if (fd[instance] == -1) { + char fname[] = "gpsN_NNN.log"; + hal.util->snprintf(fname, 13, "gps%u_%03u.log", instance+1, lognum); + fd[instance] = open(fname, O_RDONLY|O_CLOEXEC); + if (fd[instance] == -1) { + return; + } } - if (ret == 0) { - ::printf("gps rewind\n"); - lseek(temp_fd, 0, SEEK_SET); + const uint32_t magic = 0x7fe53b04; + struct { + uint32_t magic; + uint32_t time_ms; + uint32_t n; + } header; + uint8_t *buf = nullptr; + while (true) { + if (::read(fd[instance], (void *)&header, sizeof(header)) != sizeof(header) || + header.magic != magic) { + goto rewind_file; + } + if (header.time_ms+base_time[instance] > AP_HAL::millis()) { + // not ready for this data yet + ::lseek(fd[instance], -sizeof(header), SEEK_CUR); + return; + } + buf = new uint8_t[header.n]; + if (buf != nullptr && ::read(fd[instance], buf, header.n) == ssize_t(header.n)) { + write_to_autopilot((const char *)buf, header.n); + delete[] buf; + buf = nullptr; + continue; + } + goto rewind_file; } + +rewind_file: + ::printf("GPS[%u] rewind\n", unsigned(instance)); + base_time[instance] = AP_HAL::millis(); + ::lseek(fd[instance], 0, SEEK_SET); + delete[] buf; } #endif // AP_SIM_GPS_FILE_ENABLED