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

AP_NavEKF: fixed compass declination, baro cal and time shift 

now runs much faster
This commit is contained in:
Andrew Tridgell 2013-12-30 09:32:20 +11:00
parent 776cedf368
commit 050b0fb9f1
3 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -78,9 +78,10 @@ void setup()
hal.scheduler->register_delay_callback(delay_cb, 5);
ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
barometer.init();
barometer.calibrate();
compass.init();
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
@ -94,6 +95,9 @@ void setup()
ahrs.update();
}
barometer.calibrate();
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
NavEKF.InitialiseFilter();
}

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

@ -127,8 +127,11 @@ bool LogReader::update(uint8_t &type)
msg.ground_speed*1.0e-2f,
msg.ground_course*1.0e-2f,
0,
msg.num_sats);
baro.setHIL(600.0 + msg.rel_altitude*1.0e-2f);
msg.status==3?msg.num_sats:0);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
baro.setHIL((ground_alt_cm + msg.rel_altitude)*1.0e-2f);
break;
}
@ -157,8 +160,9 @@ bool LogReader::update(uint8_t &type)
void LogReader::wait_timestamp(uint32_t timestamp)
{
while (hal.scheduler->millis() < timestamp) {
hal.scheduler->delay(1);
uint32_t now = hal.scheduler->millis();
if (now < timestamp) {
hal.scheduler->time_shift(timestamp - now);
}
}

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

@ -22,6 +22,8 @@ private:
AP_Compass_HIL &compass;
GPS *&gps;
uint32_t ground_alt_cm;
uint8_t num_formats;
struct log_Format formats[32];