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

AC_WPNav: inav use _xy()

This commit is contained in:
Josh Henderson 2021-09-11 04:16:20 -04:00 committed by Andrew Tridgell
parent 77711e1505
commit 9e11f09a7f
3 changed files with 8 additions and 10 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().xy(), _center.tofloat().xy()); _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;

12
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -399,13 +399,11 @@ void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy()
_pos_control.init_xy_controller(); _pos_control.init_xy_controller();
// get current and target locations // get current and target locations
const Vector3f& curr_pos = _inav.get_position(); const Vector2f& curr_pos = _inav.get_position_xy();
// shift origin and destination horizontally // shift origin and destination horizontally
_origin.x = curr_pos.x; _origin.xy() = curr_pos;
_origin.y = curr_pos.y; _destination.xy() = curr_pos;
_destination.x = curr_pos.x;
_destination.y = curr_pos.y;
} }
/// shifts the origin and destination horizontally to the achievable stopping point /// shifts the origin and destination horizontally to the achievable stopping point
@ -561,13 +559,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
{ {
return get_horizontal_distance_cm(_inav.get_position().xy(), _destination.xy()); return get_horizontal_distance_cm(_inav.get_position_xy(), _destination.xy());
} }
/// 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().xy(), _destination.xy()); 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

4
libraries/AC_WPNav/AC_WPNav_OA.cpp
View File

@ -44,7 +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();
} }
return get_horizontal_distance_cm(_inav.get_position().xy(), _destination_oabak.xy()); return get_horizontal_distance_cm(_inav.get_position_xy(), _destination_oabak.xy());
} }
/// 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
@ -55,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().xy(), _destination_oabak.xy()); 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