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

Replay: removed attempt to interpolate IMU data 

it seems to cause more harm than good
This commit is contained in:
Andrew Tridgell 2015-05-18 09:49:06 +10:00
parent 5f963cbf82
commit 7177d5df7e
1 changed files with 9 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
Tools/Replay/Replay.pde
View File

@ -103,7 +103,6 @@ static uint16_t update_rate = 50;
static uint32_t arm_time_ms;
static bool ahrs_healthy;
static bool have_imu2;
static uint32_t last_imu_usec;
static uint8_t num_user_parameters;
static struct {
@ -321,35 +320,16 @@ static void read_sensors(const char *type)
// special handling of IMU messages as these trigger an ahrs.update()
if ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2)) {
uint32_t imu_deltat_usec = LogReader.last_timestamp_us() - last_imu_usec;
uint32_t update_delta_usec = 1e6 / update_rate;
if (imu_deltat_usec < update_delta_usec/2) {
// it is a duplicate
return;
ahrs.update();
if (ahrs.get_home().lat != 0) {
inertial_nav.update(ins.get_delta_time());
}
uint32_t update_count = (imu_deltat_usec + (update_delta_usec-1)) / update_delta_usec;
if (update_count > 8) {
update_count = 8;
}
if (update_count < 1) {
update_count = 1;
}
last_imu_usec = LogReader.last_timestamp_us();
for (uint8_t i=0; i<update_count; i++) {
ahrs.update();
if (ahrs.get_home().lat != 0) {
inertial_nav.update(ins.get_delta_time());
}
hal.scheduler->stop_clock(hal.scheduler->micros() + update_delta_usec);
dataflash.Log_Write_EKF(ahrs,false);
dataflash.Log_Write_AHRS2(ahrs);
dataflash.Log_Write_POS(ahrs);
ins.set_gyro(0, ins.get_gyro());
ins.set_accel(0, ins.get_accel());
if (ahrs.healthy() != ahrs_healthy) {
ahrs_healthy = ahrs.healthy();
printf("AHRS health: %u\n", (unsigned)ahrs_healthy);
}
dataflash.Log_Write_EKF(ahrs,false);
dataflash.Log_Write_AHRS2(ahrs);
dataflash.Log_Write_POS(ahrs);
if (ahrs.healthy() != ahrs_healthy) {
ahrs_healthy = ahrs.healthy();
printf("AHRS health: %u\n", (unsigned)ahrs_healthy);
}
}
}