Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: add comments

Browse Source
This commit is contained in:
Randy Mackay 2014-10-20 11:08:38 +09:00
parent 5e381280dc
commit 0e11189c35
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -130,40 +130,50 @@ bool AP_InertialNav::position_ok() const
return _xy_enabled; return _xy_enabled;
} }
// check_home - checks if the home position has moved and offsets everything so it still lines up
void AP_InertialNav::check_home() { void AP_InertialNav::check_home() {
if (!_xy_enabled) { if (!_xy_enabled) {
return; return;
} }
// get position move in lat, lon coordinates
int32_t lat_offset = _ahrs.get_home().lat - _last_home_lat; int32_t lat_offset = _ahrs.get_home().lat - _last_home_lat;
int32_t lng_offset = _ahrs.get_home().lng - _last_home_lng; int32_t lng_offset = _ahrs.get_home().lng - _last_home_lng;
if (lat_offset != 0) { if (lat_offset != 0) {
// calculate the position move in cm
float x_offset_cm = lat_offset * LATLON_TO_CM; float x_offset_cm = lat_offset * LATLON_TO_CM;
// move position
_position_base.x -= x_offset_cm; _position_base.x -= x_offset_cm;
_position.x -= x_offset_cm; _position.x -= x_offset_cm;
// update historic positions
for (uint8_t i = 0; i < _hist_position_estimate_x.size(); i++) { for (uint8_t i = 0; i < _hist_position_estimate_x.size(); i++) {
float &x = _hist_position_estimate_x.peek_mutable(i); float &x = _hist_position_estimate_x.peek_mutable(i);
x -= x_offset_cm; x -= x_offset_cm;
} }
// update lon scaling
_lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM; _lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
} }
if (lng_offset != 0) { if (lng_offset != 0) {
// calculate the position move in cm
float y_offset_cm = lng_offset * _lon_to_cm_scaling; float y_offset_cm = lng_offset * _lon_to_cm_scaling;
// move position
_position_base.y -= y_offset_cm; _position_base.y -= y_offset_cm;
_position.y -= y_offset_cm; _position.y -= y_offset_cm;
// update historic positions
for (uint8_t i = 0; i < _hist_position_estimate_y.size(); i++) { for (uint8_t i = 0; i < _hist_position_estimate_y.size(); i++) {
float &y = _hist_position_estimate_y.peek_mutable(i); float &y = _hist_position_estimate_y.peek_mutable(i);
y -= y_offset_cm; y -= y_offset_cm;
} }
} }
// store updated lat, lon position
_last_home_lat = _ahrs.get_home().lat; _last_home_lat = _ahrs.get_home().lat;
_last_home_lng = _ahrs.get_home().lng; _last_home_lng = _ahrs.get_home().lng;
} }