Browse Source

Copter: simplify home distance and bearing calculation

mission-4.1.18
Pierre Kancir 7 years ago committed by Randy Mackay
parent
commit
1aeb9446c0
  1. 8
      ArduCopter/navigation.cpp

8
ArduCopter/navigation.cpp

@ -14,9 +14,7 @@ void Copter::run_nav_updates(void)
uint32_t Copter::home_distance() uint32_t Copter::home_distance()
{ {
if (position_ok()) { if (position_ok()) {
const Vector3f home = pv_location_to_vector(ahrs.get_home()); _home_distance = get_distance_cm(current_loc, ahrs.get_home());
const Vector3f curr = inertial_nav.get_position();
_home_distance = get_horizontal_distance_cm(curr, home);
} }
return _home_distance; return _home_distance;
} }
@ -25,9 +23,7 @@ uint32_t Copter::home_distance()
int32_t Copter::home_bearing() int32_t Copter::home_bearing()
{ {
if (position_ok()) { if (position_ok()) {
const Vector3f home = pv_location_to_vector(ahrs.get_home()); _home_bearing = get_bearing_cd(current_loc, ahrs.get_home());
const Vector3f curr = inertial_nav.get_position();
_home_bearing = get_bearing_cd(curr,home);
} }
return _home_bearing; return _home_bearing;
} }

Loading…
Cancel
Save