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

AR_WPNav: always pivot at corners of at least WP_PIVOT_ANGLE

This commit is contained in:
Randy Mackay 2021-12-20 19:53:10 +09:00
parent 3d048ddcc2
commit b1237ffe80
2 changed files with 46 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -115,6 +115,7 @@ void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float
// ensure pivot turns are deactivated
_pivot.deactivate();
_pivot_at_next_wp = false;
// initialise origin and destination to stopping point
Location stopping_loc;
@ -180,7 +181,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
// or journey to previous waypoint was interrupted or navigation has just started
if (!_fast_waypoint) {
_pivot.deactivate();
_pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01);
_pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01, _pivot_at_next_wp);
}
// convert origin and destination to offset from EKF origin
@ -210,28 +211,33 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
}
// handle next destination
_scurve_next_leg.init();
_fast_waypoint = false;
_pivot_at_next_wp = false;
if (next_destination.initialised()) {
// convert next_destination to offset from EKF origin
Vector2f next_destination_NE;
if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) {
return false;
}
next_destination_NE *= 0.01f;
_scurve_next_leg.calculate_track(Vector3f{destination_NE.x, destination_NE.y, 0.0f},
Vector3f{next_destination_NE.x, next_destination_NE.y, 0.0f},
_pos_control.get_speed_max(),
_pos_control.get_speed_max(), // speed up (not used)
_pos_control.get_speed_max(), // speed down (not used)
_pos_control.get_accel_max(),
_pos_control.get_accel_max(), // vertical accel (not used)
1.0, // jerk time
_scurve_jerk);
// check if vehicle should pivot at next waypoint
const float next_wp_yaw_change = get_corner_angle(_origin, destination, next_destination);
_pivot_at_next_wp = _pivot.would_activate(next_wp_yaw_change);
if (!_pivot_at_next_wp) {
// convert next_destination to offset from EKF origin
Vector2f next_destination_NE;
if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) {
return false;
}
next_destination_NE *= 0.01f;
_scurve_next_leg.calculate_track(Vector3f{destination_NE.x, destination_NE.y, 0.0f},
Vector3f{next_destination_NE.x, next_destination_NE.y, 0.0f},
_pos_control.get_speed_max(),
_pos_control.get_speed_max(), // speed up (not used)
_pos_control.get_speed_max(), // speed down (not used)
_pos_control.get_accel_max(),
_pos_control.get_accel_max(), // vertical accel (not used)
1.0, // jerk time
_scurve_jerk);
// next destination provided so fast waypoint
_fast_waypoint = true;
} else {
_scurve_next_leg.init();
_fast_waypoint = false;
// next destination provided so fast waypoint
_fast_waypoint = true;
}
}
update_distance_and_bearing_to_destination();
@ -458,3 +464,17 @@ float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const
// calculate distance to target track, for reporting
return fabsf(veh_from_origin % dest_from_origin);
}
// calculate yaw change at next waypoint in degrees
// returns zero if the angle cannot be calculated because some points are on top of others
float AR_WPNav::get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const
{
// sanity check
if (!loc1.initialised() || !loc2.initialised() || !loc3.initialised()) {
return 0;
}
const float loc1_to_loc2_deg = loc1.get_bearing_to(loc2) * 0.01;
const float loc2_to_loc3_deg = loc2.get_bearing_to(loc3) * 0.01;
const float diff_yaw_deg = wrap_180(loc2_to_loc3_deg - loc1_to_loc2_deg);
return diff_yaw_deg;
}

5
libraries/AR_WPNav/AR_WPNav.h
View File

@ -116,6 +116,10 @@ protected:
// calculate the crosstrack error (does not rely on L1 controller)
float calc_crosstrack_error(const Location& current_loc) const;
// calculate yaw change at next waypoint in degrees
// returns zero if the angle cannot be calculated because some points are on top of others
float get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const;
// parameters
AP_Float _speed_max; // target speed between waypoints in m/s
AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners
@ -132,6 +136,7 @@ protected:
SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory
float _scurve_jerk; // scurve jerk max in m/s/s/s
bool _fast_waypoint; // true if vehicle will stop at the next waypoint
bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint
float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle
// variables held in vehicle code (for now)