Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: get_bearing & get_horizontal_distance use Vector2f

Browse Source
This commit is contained in:
Josh Henderson 2021-09-10 23:47:43 -04:00 committed by Andrew Tridgell
parent 5391262900
commit d5769f70ac
3 changed files with 5 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms)
target.y += - _radius * sinf(-_angle); target.y += - _radius * sinf(-_angle);
// heading is from vehicle to center of circle // heading is from vehicle to center of circle
_yaw = get_bearing_cd(_inav.get_position(), _center.tofloat()); _yaw = get_bearing_cd(_inav.get_position().xy(), _center.tofloat().xy());
if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) { if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) {
_yaw += is_positive(_rate)?-9000.0f:9000.0f; _yaw += is_positive(_rate)?-9000.0f:9000.0f;

6
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -561,15 +561,13 @@ void AC_WPNav::update_track_with_speed_accel_limits()
/// get_wp_distance_to_destination - get horizontal distance to destination in cm /// get_wp_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_wp_distance_to_destination() const float AC_WPNav::get_wp_distance_to_destination() const
{ {
// get current location return get_horizontal_distance_cm(_inav.get_position().xy(), _destination.xy());
const Vector3f &curr = _inav.get_position();
return norm(_destination.x-curr.x,_destination.y-curr.y);
} }
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_wp_bearing_to_destination() const int32_t AC_WPNav::get_wp_bearing_to_destination() const
{ {
return get_bearing_cd(_inav.get_position(), _destination); return get_bearing_cd(_inav.get_position().xy(), _destination.xy());
} }
/// update_wpnav - run the wp controller - should be called at 100hz or higher /// update_wpnav - run the wp controller - should be called at 100hz or higher

6
libraries/AC_WPNav/AC_WPNav_OA.cpp
View File

@ -44,9 +44,7 @@ float AC_WPNav_OA::get_wp_distance_to_destination() const
return AC_WPNav::get_wp_distance_to_destination(); return AC_WPNav::get_wp_distance_to_destination();
} }
// get current location return get_horizontal_distance_cm(_inav.get_position().xy(), _destination_oabak.xy());
const Vector3f &curr = _inav.get_position();
return norm(_destination_oabak.x-curr.x, _destination_oabak.y-curr.y);
} }
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
@ -57,7 +55,7 @@ int32_t AC_WPNav_OA::get_wp_bearing_to_destination() const
return AC_WPNav::get_wp_bearing_to_destination(); return AC_WPNav::get_wp_bearing_to_destination();
} }
return get_bearing_cd(_inav.get_position(), _destination_oabak); return get_bearing_cd(_inav.get_position().xy(), _destination_oabak.xy());
} }
/// true when we have come within RADIUS cm of the waypoint /// true when we have come within RADIUS cm of the waypoint