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

AP_InertialNav: update properly if home position moves

This commit is contained in:
Jonathan Challinger 2014-10-17 11:39:52 -07:00 committed by Randy Mackay
parent 5fc02bdbc4
commit 5e381280dc
2 changed files with 51 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -47,6 +47,9 @@ void AP_InertialNav::update(float dt)
// check if new baro readings have arrived and use them to correct vertical accelerometer offsets.
check_baro();
// check if home has moved and update
check_home();
// check if new gps readings have arrived and use them to correct position estimates
check_gps();
@ -127,6 +130,44 @@ bool AP_InertialNav::position_ok() const
return _xy_enabled;
}
void AP_InertialNav::check_home() {
if (!_xy_enabled) {
return;
}
int32_t lat_offset = _ahrs.get_home().lat - _last_home_lat;
int32_t lng_offset = _ahrs.get_home().lng - _last_home_lng;
if (lat_offset != 0) {
float x_offset_cm = lat_offset * LATLON_TO_CM;
_position_base.x -= x_offset_cm;
_position.x -= x_offset_cm;
for (uint8_t i = 0; i < _hist_position_estimate_x.size(); i++) {
float &x = _hist_position_estimate_x.peek_mutable(i);
x -= x_offset_cm;
}
_lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
}
if (lng_offset != 0) {
float y_offset_cm = lng_offset * _lon_to_cm_scaling;
_position_base.y -= y_offset_cm;
_position.y -= y_offset_cm;
for (uint8_t i = 0; i < _hist_position_estimate_y.size(); i++) {
float &y = _hist_position_estimate_y.peek_mutable(i);
y -= y_offset_cm;
}
}
_last_home_lat = _ahrs.get_home().lat;
_last_home_lng = _ahrs.get_home().lng;
}
// check_gps - check if new gps readings have arrived and use them to correct position estimates
void AP_InertialNav::check_gps()
{
@ -243,6 +284,8 @@ void AP_InertialNav::setup_home_position(void)
_position_correction.y = 0.0f;
_position.x = 0.0f;
_position.y = 0.0f;
_last_home_lat = _ahrs.get_home().lat;
_last_home_lng = _ahrs.get_home().lng;
// clear historic estimates
_hist_position_estimate_x.clear();

8
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -213,6 +213,11 @@ protected:
*/
void correct_with_gps(uint32_t now, int32_t lon, int32_t lat);
/**
* check_home - checks if the home position has moved and offsets everything so it still lines up
*/
void check_home();
/**
* check_gps - checks if new gps readings have arrived and calls correct_with_gps to
* calculate the horizontal position error
@ -297,6 +302,9 @@ protected:
Baro_Glitch& _baro_glitch; // Baro glitch detector
uint8_t _error_count; // number of missed GPS updates
int32_t _last_home_lat;
int32_t _last_home_lng;
};
#if AP_AHRS_NAVEKF_AVAILABLE