diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index fb30863f56..6dac14085e 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -4,7 +4,6 @@ extern const AP_HAL::HAL& hal; // maximum velocities and accelerations -#define WPNAV_ACCELERATION 250.0f // maximum horizontal acceleration in cm/s/s that wp navigation will request #define WPNAV_WP_SPEED 1000.0f // default horizontal speed between waypoints in cm/s #define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s #define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm @@ -147,12 +146,8 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const /// 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) { - - // sanity check parameters - // check _wp_accel_cmss is reasonable + // set corner acceleration based on maximum lean angle _scurve_accel_corner = angle_to_accel(_pos_control.get_lean_angle_max_cd() * 0.01) * 100; - const float wp_accel_cmss = MIN(_wp_accel_cmss, _scurve_accel_corner); - _wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss); // check _wp_radius_cm is reasonable _wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN)); @@ -169,14 +164,14 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) _wp_desired_speed_xy_cms = MAX(_wp_desired_speed_xy_cms, WPNAV_WP_SPEED_MIN); // initialise position controller speed and acceleration - _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); - _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); + _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, get_wp_acceleration()); + _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, get_wp_acceleration()); _pos_control.set_max_speed_accel_z(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss); _pos_control.set_correction_speed_accel_z(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss); // calculate scurve jerk and jerk time if (!is_positive(_wp_jerk)) { - _wp_jerk = _wp_accel_cmss; + _wp_jerk.set(get_wp_acceleration()); } calc_scurve_jerk_and_snap(); @@ -217,8 +212,8 @@ void AC_WPNav::set_speed_xy(float speed_cms) _wp_desired_speed_xy_cms = speed_cms; // update position controller speed and acceleration - _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); - _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); + _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, get_wp_acceleration()); + _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, get_wp_acceleration()); // change track speed update_track_with_speed_accel_limits(); @@ -338,7 +333,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) } else { _scurve_this_leg.calculate_track(_origin, _destination, _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - _wp_accel_cmss, _wp_accel_z_cmss, + get_wp_acceleration(), _wp_accel_z_cmss, _scurve_snap * 100.0f, _scurve_jerk * 100.0f); if (!is_zero(origin_speed)) { // rebuild start of scurve if we have a non-zero origin speed @@ -366,7 +361,7 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain _scurve_next_leg.calculate_track(_destination, destination, _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - _wp_accel_cmss, _wp_accel_z_cmss, + get_wp_acceleration(), _wp_accel_z_cmss, _scurve_snap * 100.0f, _scurve_jerk * 100.0); if (_this_leg_is_spline) { const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max(); @@ -485,7 +480,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) if (is_positive(_wp_desired_speed_xy_cms)) { update_vel_accel(_offset_vel, _offset_accel, dt, 0.0, 0.0); const float vel_input = !_paused ? _wp_desired_speed_xy_cms * offset_z_scaler : 0.0; - shape_vel_accel(vel_input, 0.0, _offset_vel, _offset_accel, -_wp_accel_cmss, _wp_accel_cmss, + shape_vel_accel(vel_input, 0.0, _offset_vel, _offset_accel, -get_wp_acceleration(), get_wp_acceleration(), _pos_control.get_shaping_jerk_xy_cmsss(), dt, true); vel_scaler_dt = _offset_vel / _wp_desired_speed_xy_cms; } @@ -493,7 +488,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) // change s-curve time speed with a time constant of maximum acceleration / maximum jerk float track_scaler_tc = 1.0f; if (!is_zero(_wp_jerk)) { - track_scaler_tc = 0.01f * _wp_accel_cmss/_wp_jerk; + track_scaler_tc = 0.01f * get_wp_acceleration()/_wp_jerk; } _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc); @@ -556,7 +551,7 @@ void AC_WPNav::update_track_with_speed_accel_limits() // update this leg if (_this_leg_is_spline) { _spline_this_leg.set_speed_accel(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - _wp_accel_cmss, _wp_accel_z_cmss); + get_wp_acceleration(), _wp_accel_z_cmss); } else { _scurve_this_leg.set_speed_max(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms()); } @@ -564,7 +559,7 @@ void AC_WPNav::update_track_with_speed_accel_limits() // update next leg if (_next_leg_is_spline) { _spline_next_leg.set_speed_accel(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - _wp_accel_cmss, _wp_accel_z_cmss); + get_wp_acceleration(), _wp_accel_z_cmss); } else { _scurve_next_leg.set_speed_max(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms()); } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 13fdc4e171..cab29d8757 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -12,6 +12,9 @@ #include #include // Stop at fence library +// maximum velocities and accelerations +#define WPNAV_ACCELERATION 250.0f // maximum horizontal acceleration in cm/s/s that wp navigation will request + class AC_WPNav { public: @@ -81,7 +84,7 @@ public: float get_accel_z() const { return _wp_accel_z_cmss; } /// get_wp_acceleration - returns acceleration in cm/s/s during missions - float get_wp_acceleration() const { return _wp_accel_cmss.get(); } + float get_wp_acceleration() const { return (is_positive(_wp_accel_cmss)) ? _wp_accel_cmss : WPNAV_ACCELERATION; } /// get_wp_destination waypoint using position vector /// x,y are distance from ekf origin in cm