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

AC_WPNav: remove _wp_accel_cmss.set_and_save_ifchanged

This commit is contained in:
Leonard Hall 2022-12-30 00:28:12 +10:30 committed by Randy Mackay
parent 4ab1153c4a
commit 0af7cd59e1
2 changed files with 16 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -4,7 +4,6 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// maximum velocities and accelerations // 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 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_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s
#define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm #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 /// 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) void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
{ {
// set corner acceleration based on maximum lean angle
// sanity check parameters
// check _wp_accel_cmss is reasonable
_scurve_accel_corner = angle_to_accel(_pos_control.get_lean_angle_max_cd() * 0.01) * 100; _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 // check _wp_radius_cm is reasonable
_wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN)); _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); _wp_desired_speed_xy_cms = MAX(_wp_desired_speed_xy_cms, WPNAV_WP_SPEED_MIN);
// initialise position controller speed and acceleration // initialise position controller speed and acceleration
_pos_control.set_max_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, _wp_accel_cmss); _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_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); _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 // calculate scurve jerk and jerk time
if (!is_positive(_wp_jerk)) { if (!is_positive(_wp_jerk)) {
_wp_jerk = _wp_accel_cmss; _wp_jerk.set(get_wp_acceleration());
} }
calc_scurve_jerk_and_snap(); 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; _wp_desired_speed_xy_cms = speed_cms;
// update position controller speed and acceleration // update position controller speed and acceleration
_pos_control.set_max_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, _wp_accel_cmss); _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, get_wp_acceleration());
// change track speed // change track speed
update_track_with_speed_accel_limits(); update_track_with_speed_accel_limits();
@ -338,7 +333,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
} else { } else {
_scurve_this_leg.calculate_track(_origin, _destination, _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(), _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); _scurve_snap * 100.0f, _scurve_jerk * 100.0f);
if (!is_zero(origin_speed)) { if (!is_zero(origin_speed)) {
// rebuild start of scurve if we have a non-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, _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(), _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); _scurve_snap * 100.0f, _scurve_jerk * 100.0);
if (_this_leg_is_spline) { if (_this_leg_is_spline) {
const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max(); 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)) { if (is_positive(_wp_desired_speed_xy_cms)) {
update_vel_accel(_offset_vel, _offset_accel, dt, 0.0, 0.0); 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; 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); _pos_control.get_shaping_jerk_xy_cmsss(), dt, true);
vel_scaler_dt = _offset_vel / _wp_desired_speed_xy_cms; 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 // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
float track_scaler_tc = 1.0f; float track_scaler_tc = 1.0f;
if (!is_zero(_wp_jerk)) { 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); _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 // update this leg
if (_this_leg_is_spline) { 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(), _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 { } 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()); _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 // update next leg
if (_next_leg_is_spline) { 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(), _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 { } 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()); _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());
} }

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -12,6 +12,9 @@
#include <AP_Terrain/AP_Terrain.h> #include <AP_Terrain/AP_Terrain.h>
#include <AC_Avoidance/AC_Avoid.h> // Stop at fence library #include <AC_Avoidance/AC_Avoid.h> // 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 class AC_WPNav
{ {
public: public:
@ -81,7 +84,7 @@ public:
float get_accel_z() const { return _wp_accel_z_cmss; } float get_accel_z() const { return _wp_accel_z_cmss; }
/// get_wp_acceleration - returns acceleration in cm/s/s during missions /// 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 /// get_wp_destination waypoint using position vector
/// x,y are distance from ekf origin in cm /// x,y are distance from ekf origin in cm