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

AP_NavEKF: support 400Hz replay from 50Hz logs

Browse Source
This commit is contained in:
Andrew Tridgell 2014-02-26 19:34:01 +11:00
parent 3fa2207a2d
commit 66f238c0da
2 changed files with 45 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
View File

@ -43,6 +43,7 @@
#include <AP_InertialNav.h> #include <AP_InertialNav.h>
#include <AP_NavEKF.h> #include <AP_NavEKF.h>
#include <stdio.h> #include <stdio.h>
#include <getopt.h>
#include "LogReader.h" #include "LogReader.h"
@ -76,6 +77,7 @@ static FILE *ekf4f;
static bool done_baro_init; static bool done_baro_init;
static bool done_home_init; static bool done_home_init;
static uint16_t update_rate;
void setup() void setup()
{ {
@ -84,12 +86,29 @@ void setup()
const char *filename = "log.bin"; const char *filename = "log.bin";
uint8_t argc; uint8_t argc;
char * const *argv; char * const *argv;
int opt;
hal.util->commandline_arguments(argc, argv); hal.util->commandline_arguments(argc, argv);
if (argc > 1) {
filename = argv[1]; while ((opt = getopt(argc, argv, "r:")) != -1) {
switch (opt) {
case 'r':
update_rate = strtol(optarg, NULL, 0);
break;
}
}
argv += optind;
argc -= optind;
if (argc > 0) {
filename = argv[0];
} }
hal.console->printf("Processing log %s\n", filename); hal.console->printf("Processing log %s\n", filename);
if (update_rate != 0) {
hal.console->printf("Using an update rate of %u Hz\n", update_rate);
}
LogReader.open_log(filename); LogReader.open_log(filename);
LogReader.wait_type(LOG_GPS_MSG); LogReader.wait_type(LOG_GPS_MSG);
@ -107,7 +126,21 @@ void setup()
barometer.read(); barometer.read();
compass.init(); compass.init();
inertial_nav.init(); inertial_nav.init();
switch (update_rate) {
case 0:
case 50:
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ); ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
break;
case 100:
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_100HZ);
break;
case 200:
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_200HZ);
break;
case 400:
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_400HZ);
break;
}
plotf = fopen("plot.dat", "w"); plotf = fopen("plot.dat", "w");
plotf2 = fopen("plot2.dat", "w"); plotf2 = fopen("plot2.dat", "w");
@ -159,10 +192,15 @@ static void read_sensors(uint8_t type)
ahrs.estimate_wind(); ahrs.estimate_wind();
} }
} else if (type == LOG_IMU_MSG) { } else if (type == LOG_IMU_MSG) {
uint32_t update_delta_usec = 1e6 / update_rate;
uint8_t update_count = update_rate>0?update_rate/50:1;
for (uint8_t i=0; i<update_count; i++) {
hal.scheduler->stop_clock(hal.scheduler->micros() + i*update_delta_usec);
ahrs.update(); ahrs.update();
if (ahrs.get_home().lat != 0) { if (ahrs.get_home().lat != 0) {
inertial_nav.update(ins.get_delta_time()); inertial_nav.update(ins.get_delta_time());
} }
}
} else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) || } else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
(type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) { (type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {
compass.read(); compass.read();

2
libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
View File

@ -341,7 +341,7 @@ bool LogReader::update(uint8_t &type)
void LogReader::wait_timestamp(uint32_t timestamp) void LogReader::wait_timestamp(uint32_t timestamp)
{ {
hal.scheduler->stop_clock(timestamp); hal.scheduler->stop_clock(timestamp*1000UL);
} }
bool LogReader::wait_type(uint8_t wtype) bool LogReader::wait_type(uint8_t wtype)