Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: support playback of new GPS log format

This commit is contained in:
Andrew Tridgell 2022-10-15 09:38:45 +11:00
parent 6f805cb537
commit 4b8b2d8236
1 changed files with 43 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
libraries/SITL/SIM_GPS.cpp
View File

@ -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