Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: improve GPS debug logging

Browse Source 
use timestamped data allowing for much more precise playback
This commit is contained in:
Andrew Tridgell 2022-10-15 09:33:43 +11:00
parent afe7790e4b
commit 5cd1aeaf38
2 changed files with 68 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -418,34 +418,73 @@ bad_yaw:
#if AP_GPS_DEBUG_LOGGING_ENABLED
// log some data for debugging
/*
log some data for debugging
the logging format matches that used by SITL with SIM_GPS_TYPE=7,
allowing for development of GPS drivers based on logged data
*/
void AP_GPS_Backend::log_data(const uint8_t *data, uint16_t length)
{
logging.buf.write(data, length);
if (!logging.io_registered) {
logging.io_registered = true;
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_GPS_Backend::logging_update, void));
if (state.instance < 2) {
logging[state.instance].buf.write(data, length);
}
if (!log_thread_created) {
log_thread_created = true;
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_GPS_Backend::logging_start, void), "gps_log", 4096, AP_HAL::Scheduler::PRIORITY_IO, 0);
}
}
// IO thread update, writing to log file
void AP_GPS_Backend::logging_update(void)
AP_GPS_Backend::loginfo AP_GPS_Backend::logging[2];
bool AP_GPS_Backend::log_thread_created;
// logging loop, needs to be static to allow for re-alloc of GPS backends
void AP_GPS_Backend::logging_loop(void)
{
if (logging.fd == -1) {
char fname[] = "gpsN.log";
fname[3] = '1' + state.instance;
logging.fd = AP::FS().open(fname, O_WRONLY|O_CREAT|O_APPEND);
while (true) {
hal.scheduler->delay(10);
static uint16_t lognum;
for (uint8_t instance=0; instance<2; instance++) {
if (logging[instance].fd == -1) {
char fname[] = "gpsN_XXX.log";
fname[3] = '1' + instance;
if (lognum == 0) {
for (lognum=1; lognum<1000; lognum++) {
struct stat st;
hal.util->snprintf(&fname[5], 8, "%03u.log", lognum);
if (AP::FS().stat(fname, &st) != 0) {
break;
}
if (logging.fd != -1) {
}
}
hal.util->snprintf(&fname[5], 8, "%03u.log", lognum);
logging[instance].fd = AP::FS().open(fname, O_WRONLY|O_CREAT|O_APPEND);
}
if (logging[instance].fd != -1) {
uint32_t n = 0;
const uint8_t *p = logging.buf.readptr(n);
if (p != nullptr && n != 0) {
int32_t written = AP::FS().write(logging.fd, p, n);
if (written > 0) {
logging.buf.advance(written);
AP::FS().fsync(logging.fd);
const uint8_t *p;
while ((p = logging[instance].buf.readptr(n)) != nullptr && n != 0) {
struct {
uint32_t magic = 0x7fe53b04U;
uint32_t time_ms;
uint32_t n;
} header;
header.n = n;
header.time_ms = AP_HAL::millis();
// short writes are unlikely and are ignored (only FS full errors)
AP::FS().write(logging[instance].fd, (const uint8_t *)&header, sizeof(header));
AP::FS().write(logging[instance].fd, p, n);
logging[instance].buf.advance(n);
AP::FS().fsync(logging[instance].fd);
}
}
}
}
}
// logging thread start, needs to be non-static for thread_create
void AP_GPS_Backend::logging_start(void)
{
logging_loop();
}
#endif // AP_GPS_DEBUG_LOGGING_ENABLED

14
libraries/AP_GPS/GPS_Backend.h
View File

@ -161,12 +161,16 @@ private:
uint32_t _rate_ms;
uint32_t _last_rate_ms;
uint16_t _rate_counter;
#if AP_GPS_DEBUG_LOGGING_ENABLED
struct {
// support raw GPS logging
static struct loginfo {
int fd = -1;
ByteBuffer buf{32768};
bool io_registered;
} logging;
void logging_update(void);
ByteBuffer buf{16000};
} logging[2];
static bool log_thread_created;
static void logging_loop(void);
void logging_start(void);
#endif
};