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

Copter: fix home_distance calculation 

Distance was calculated vs EKF origin instead of AHRS home
This commit is contained in:
Randy Mackay 2015-07-08 15:22:16 +09:00
parent 290cdcf6fb
commit 2e6b6e473a
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/navigation.cpp
View File

@ -62,12 +62,11 @@ void Copter::calc_wp_bearing()
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
void Copter::calc_home_distance_and_bearing()
{
Vector3f curr = inertial_nav.get_position();
// calculate home distance and bearing
if (position_ok()) {
home_distance = pythagorous2(curr.x, curr.y);
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing