aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 committed by Randy Mackay
parent 2c6ccb3e05
commit 12a273f376
2 changed files with 68 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -418,34 +418,73 @@ bad_yaw:
#if AP_GPS_DEBUG_LOGGING_ENABLED #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) void AP_GPS_Backend::log_data(const uint8_t *data, uint16_t length)
{ {
logging.buf.write(data, length); if (state.instance < 2) {
if (!logging.io_registered) { logging[state.instance].buf.write(data, length);
logging.io_registered = true; }
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_GPS_Backend::logging_update, void)); 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 AP_GPS_Backend::loginfo AP_GPS_Backend::logging[2];
void AP_GPS_Backend::logging_update(void) 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) { while (true) {
char fname[] = "gpsN.log"; hal.scheduler->delay(10);
fname[3] = '1' + state.instance; static uint16_t lognum;
logging.fd = AP::FS().open(fname, O_WRONLY|O_CREAT|O_APPEND); for (uint8_t instance=0; instance<2; instance++) {
} if (logging[instance].fd == -1) {
if (logging.fd != -1) { char fname[] = "gpsN_XXX.log";
uint32_t n = 0; fname[3] = '1' + instance;
const uint8_t *p = logging.buf.readptr(n); if (lognum == 0) {
if (p != nullptr && n != 0) { for (lognum=1; lognum<1000; lognum++) {
int32_t written = AP::FS().write(logging.fd, p, n); struct stat st;
if (written > 0) { hal.util->snprintf(&fname[5], 8, "%03u.log", lognum);
logging.buf.advance(written); if (AP::FS().stat(fname, &st) != 0) {
AP::FS().fsync(logging.fd); break;
}
}
}
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;
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 #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 _rate_ms;
uint32_t _last_rate_ms; uint32_t _last_rate_ms;
uint16_t _rate_counter; uint16_t _rate_counter;
#if AP_GPS_DEBUG_LOGGING_ENABLED #if AP_GPS_DEBUG_LOGGING_ENABLED
struct { // support raw GPS logging
static struct loginfo {
int fd = -1; int fd = -1;
ByteBuffer buf{32768}; ByteBuffer buf{16000};
bool io_registered; } logging[2];
} logging; static bool log_thread_created;
void logging_update(void); static void logging_loop(void);
void logging_start(void);
#endif #endif
}; };