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

AP_NavEKF: support 400Hz replay from 50Hz logs

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

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

@ -43,6 +43,7 @@
#include <AP_InertialNav.h>
#include <AP_NavEKF.h>
#include <stdio.h>
#include <getopt.h>
#include "LogReader.h"
@ -76,6 +77,7 @@ static FILE *ekf4f;
static bool done_baro_init;
static bool done_home_init;
static uint16_t update_rate;
void setup()
{
@ -84,12 +86,29 @@ void setup()
const char *filename = "log.bin";
uint8_t argc;
char * const *argv;
int opt;
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);
if (update_rate != 0) {
hal.console->printf("Using an update rate of %u Hz\n", update_rate);
}
LogReader.open_log(filename);
LogReader.wait_type(LOG_GPS_MSG);
@ -107,7 +126,21 @@ void setup()
barometer.read();
compass.init();
inertial_nav.init();
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
switch (update_rate) {
case 0:
case 50:
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");
plotf2 = fopen("plot2.dat", "w");
@ -159,9 +192,14 @@ static void read_sensors(uint8_t type)
ahrs.estimate_wind();
}
} else if (type == LOG_IMU_MSG) {
ahrs.update();
if (ahrs.get_home().lat != 0) {
inertial_nav.update(ins.get_delta_time());
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();
if (ahrs.get_home().lat != 0) {
inertial_nav.update(ins.get_delta_time());
}
}
} else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
(type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {

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)
{
hal.scheduler->stop_clock(timestamp);
hal.scheduler->stop_clock(timestamp*1000UL);
}
bool LogReader::wait_type(uint8_t wtype)