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

AC_WPNav: Add corner acceleration limit parameter

This commit is contained in:
Leonard Hall 2022-12-30 00:37:21 +10:30 committed by Randy Mackay
parent 638379d3ed
commit ed24a635c6
2 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -92,6 +92,15 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @User: Advanced
AP_GROUPINFO("TER_MARGIN", 12, AC_WPNav, _terrain_margin, 10.0),
// @Param: ACCEL_C
// @DisplayName: Waypoint Cornering Acceleration
// @Description: Defines the maximum cornering acceleration in cm/s/s used during missions, zero uses max lean angle.
// @Units: cm/s/s
// @Range: 0 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("ACCEL_C", 13, AC_WPNav, _wp_accel_c_cmss, 0.0),
AP_GROUPEND
};
@ -145,10 +154,7 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const
/// stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero
/// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
{
// set corner acceleration based on maximum lean angle
_scurve_accel_corner = angle_to_accel(_pos_control.get_lean_angle_max_cd() * 0.01) * 100;
{
// check _wp_radius_cm is reasonable
_wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN));
@ -499,7 +505,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
if (!_this_leg_is_spline) {
// update target position, velocity and acceleration
target_pos = _origin;
s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel, target_accel);
s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, get_corner_acceleration(), _flags.fast_waypoint, _track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel, target_accel);
} else {
// splinetarget_vel
target_vel = curr_target_vel;

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -86,6 +86,9 @@ public:
/// get_wp_acceleration - returns acceleration in cm/s/s during missions
float get_wp_acceleration() const { return (is_positive(_wp_accel_cmss)) ? _wp_accel_cmss : WPNAV_ACCELERATION; }
/// get_wp_acceleration - returns acceleration in cm/s/s during missions
float get_corner_acceleration() const { return (is_positive(_wp_accel_c_cmss)) ? _wp_accel_cmss : get_wp_acceleration(); }
/// get_wp_destination waypoint using position vector
/// x,y are distance from ekf origin in cm
/// z may be cm above ekf origin or terrain (see origin_and_destination_are_terrain_alt method)
@ -238,6 +241,7 @@ protected:
AP_Float _wp_speed_down_cms; // default maximum descent rate in cm/s
AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions
AP_Float _wp_accel_c_cmss; // cornering acceleration in cm/s/s during missions
AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions
AP_Float _wp_jerk; // maximum jerk used to generate scurve trajectories in m/s/s/s
AP_Float _terrain_margin; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin
@ -250,7 +254,6 @@ protected:
SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory
SCurve _scurve_this_leg; // current scurve trajectory
SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory
float _scurve_accel_corner; // scurve maximum corner acceleration in m/s/s
float _scurve_jerk; // scurve jerk max in m/s/s/s
float _scurve_snap; // scurve snap in m/s/s/s/s