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

AP_NavEKF: fixed compass declination, baro cal and time shift

Browse Source 
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); hal.scheduler->register_delay_callback(delay_cb, 5);
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
ahrs.set_fly_forward(true);
ahrs.set_wind_estimation(true);
barometer.init(); barometer.init();
barometer.calibrate();
compass.init(); compass.init();
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ); ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
@ -94,6 +95,9 @@ void setup()
ahrs.update(); ahrs.update();
} }
barometer.calibrate();
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
NavEKF.InitialiseFilter(); 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_speed*1.0e-2f,
msg.ground_course*1.0e-2f, msg.ground_course*1.0e-2f,
0, 0,
msg.num_sats); msg.status==3?msg.num_sats:0);
baro.setHIL(600.0 + msg.rel_altitude*1.0e-2f); 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; break;
} }
@ -157,8 +160,9 @@ bool LogReader::update(uint8_t &type)
void LogReader::wait_timestamp(uint32_t timestamp) void LogReader::wait_timestamp(uint32_t timestamp)
{ {
while (hal.scheduler->millis() < timestamp) { uint32_t now = hal.scheduler->millis();
hal.scheduler->delay(1); 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; AP_Compass_HIL &compass;
GPS *&gps; GPS *&gps;
uint32_t ground_alt_cm;
uint8_t num_formats; uint8_t num_formats;
struct log_Format formats[32]; struct log_Format formats[32];