aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AR_WPNav: apply_speed_min used for overshoot limits

Browse Source 
speed_min is applied to overshoot_speed_max only instead of des_speed_lim
so that the target doesn't immediately jump to the minimum.
also constify apply_speed_min
This commit is contained in:
Randy Mackay 2020-12-07 13:47:59 +09:00 committed by Peter Barker
parent 62a02f7c87
commit a2fe846a5f
2 changed files with 9 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -421,12 +421,11 @@ void AR_WPNav::update_desired_speed(float dt)
} }
// calculate and limit speed to allow vehicle to stay on circle // calculate and limit speed to allow vehicle to stay on circle
const float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m)); // ensure limit does not force speed below minimum
float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m));
apply_speed_min(overshoot_speed_max);
des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max); des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max);
// ensure speed does not fall below minimum
apply_speed_min(des_speed_lim);
// limit speed based on distance to waypoint and max acceleration/deceleration // limit speed based on distance to waypoint and max acceleration/deceleration
if (is_positive(_oa_distance_to_destination) && is_positive(_atc.get_decel_max())) { if (is_positive(_oa_distance_to_destination) && is_positive(_atc.get_decel_max())) {
const float dist_speed_max = safe_sqrt(2.0f * _oa_distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final)); const float dist_speed_max = safe_sqrt(2.0f * _oa_distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final));
@ -445,18 +444,15 @@ void AR_WPNav::set_turn_params(float turn_max_g, float turn_radius, bool pivot_p
} }
// adjust speed to ensure it does not fall below value held in SPEED_MIN // adjust speed to ensure it does not fall below value held in SPEED_MIN
void AR_WPNav::apply_speed_min(float &desired_speed) // desired_speed should always be positive (or zero)
void AR_WPNav::apply_speed_min(float &desired_speed) const
{ {
if (!is_positive(_speed_min)) { if (!is_positive(_speed_min) || (_speed_min > _speed_max)) {
return; return;
} }
float speed_min = MIN(_speed_min, _speed_max);
// ensure speed does not fall below minimum // ensure speed does not fall below minimum
if (fabsf(desired_speed) < speed_min) { desired_speed = MAX(desired_speed, _speed_min);
desired_speed = is_negative(desired_speed) ? -speed_min : speed_min;
}
} }
// calculate the crosstrack error (does not rely on L1 controller) // calculate the crosstrack error (does not rely on L1 controller)

3
libraries/AR_WPNav/AR_WPNav.h
View File

@ -110,7 +110,8 @@ private:
void update_pivot_active_flag(); void update_pivot_active_flag();
// adjust speed to ensure it does not fall below value held in SPEED_MIN // adjust speed to ensure it does not fall below value held in SPEED_MIN
void apply_speed_min(float &desired_speed); // desired_speed should always be positive (or zero)
void apply_speed_min(float &desired_speed) const;
// calculate the crosstrack error (does not rely on L1 controller) // calculate the crosstrack error (does not rely on L1 controller)
float calc_crosstrack_error(const Location& current_loc) const; float calc_crosstrack_error(const Location& current_loc) const;