Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AC_WPNav: Add corner acceleration limit parameter #22431

Merged
merged 2 commits into from
Dec 30, 2022
Merged

AC_WPNav: Add corner acceleration limit parameter #22431

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ed791da
AC_WPNav: Add corner acceleration limit parameter
lthall Dec 29, 2022
aa4fca6
AutoTest: Increase mission timeout to account for lower corner accele…
lthall Dec 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Tools/autotest/common.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6598,7 +6598,7 @@ def assert_current_waypoint(self, wpnum):
if seq != wpnum:
raise NotAchievedException("Incorrect current wp")

def wait_current_waypoint(self, wpnum, timeout=60):
def wait_current_waypoint(self, wpnum, timeout=70):
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > timeout:
Expand Down
16 changes: 11 additions & 5 deletions libraries/AC_WPNav/AC_WPNav.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should remove the "Defines the" part because all parameters define something or configure something. This is not a blocker though.

// @Units: cm/s/s
lthall marked this conversation as resolved.
Show resolved Hide resolved
// @Range: 0 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("ACCEL_C", 13, AC_WPNav, _wp_accel_c_cmss, 0.0),

AP_GROUPEND
};

Expand Down Expand Up @@ -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));

Expand Down Expand Up @@ -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;
Expand Down
5 changes: 4 additions & 1 deletion libraries/AC_WPNav/AC_WPNav.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,9 @@ class AC_WPNav
/// 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)
Expand Down Expand Up @@ -238,6 +241,7 @@ class AC_WPNav
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
Expand All @@ -250,7 +254,6 @@ class AC_WPNav
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

Expand Down