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.

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

Make speed changes during missions obey WPNAV_ACCEL #12821

Open
wants to merge 1 commit into
base: master
from
Open
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -119,6 +119,7 @@ void AC_WPNav::wp_and_spline_init()
_pos_control.set_desired_velocity_xy(0.0f, 0.0f);

// initialise position controller speed and acceleration
_wp_desired_speed_cms = _wp_speed_cms;
_pos_control.set_max_speed_xy(_wp_speed_cms);
_pos_control.set_max_accel_xy(_wp_accel_cmss);
_pos_control.set_max_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
@@ -133,11 +134,9 @@ void AC_WPNav::wp_and_spline_init()
/// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
void AC_WPNav::set_speed_xy(float speed_cms)
{
// range check new target speed and update position controller
// range check target speed
if (speed_cms >= WPNAV_WP_SPEED_MIN) {
_pos_control.set_max_speed_xy(speed_cms);
// flag that wp leash must be recalculated
_flags.recalc_wp_leash = true;
_wp_desired_speed_cms = speed_cms;
}
}

@@ -494,6 +493,9 @@ bool AC_WPNav::update_wpnav()
_pos_control.set_max_accel_xy(_wp_accel_cmss);
_pos_control.set_max_accel_z(_wp_accel_z_cmss);

// wp_speed_update - update _pos_control.set_max_speed_xy if speed change has been requested
wp_speed_update(dt);

// advance the target if necessary
if (!advance_wp_target_along_track(dt)) {
// To-Do: handle inability to advance along track (probably because of missing terrain data)
@@ -783,6 +785,9 @@ bool AC_WPNav::update_spline()
// get dt from pos controller
float dt = _pos_control.get_dt();

// wp_speed_update - update _pos_control.set_max_speed_xy if speed change has been requested
wp_speed_update(dt);

// advance the target if necessary
if (!advance_spline_target_along_track(dt)) {
// To-Do: handle failure to advance along track (due to missing terrain data)
@@ -1031,3 +1036,32 @@ float AC_WPNav::get_slow_down_speed(float dist_from_dest_cm, float accel_cmss)
return target_speed;
}
}

/// wp_speed_update - calculates how to handle speed change requests
void AC_WPNav::wp_speed_update(float dt)
{
// return if speed has not changed
if (is_equal(_wp_desired_speed_cms, _wp_current_speed_cms)) {
return;
}
// calculate speed change
if (_wp_desired_speed_cms > _wp_current_speed_cms) {
// speed up is requested so increase speed within limit set by WPNAV_ACCEL
_wp_current_speed_cms += _wp_accel_cmss * dt;
if (_wp_current_speed_cms > _wp_desired_speed_cms) {
_wp_current_speed_cms = _wp_desired_speed_cms;
}
} else if (_wp_desired_speed_cms < _wp_current_speed_cms) {
// slow down is requested so reduce speed within limit set by WPNAV_ACCEL
_wp_current_speed_cms -= _wp_accel_cmss * dt;
if (_wp_current_speed_cms < _wp_desired_speed_cms) {
_wp_current_speed_cms = _wp_desired_speed_cms;
}
}

//update position controller speed
_pos_control.set_max_speed_xy(_wp_current_speed_cms);

// flag that wp leash must be recalculated
_flags.recalc_wp_leash = true;
}
@@ -247,6 +247,9 @@ class AC_WPNav

/// get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm)
float get_slow_down_speed(float dist_from_dest_cm, float accel_cmss);

/// wp_speed_update - calculates how to change speed when changes are requested
void wp_speed_update(float dt);

/// spline protected functions

@@ -288,6 +291,8 @@ class AC_WPNav

// waypoint controller internal variables
uint32_t _wp_last_update; // time of last update_wpnav call
float _wp_current_speed_cms; // current wp speed in cm/sec
float _wp_desired_speed_cms; // desired wp speed in cm/sec
Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin
Vector3f _destination; // target destination in cm from ekf origin
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.