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

AR_PosControl: no min speed when stopping

This commit is contained in:
Randy Mackay 2023-11-15 21:40:42 +09:00 committed by Andrew Tridgell
parent fa6a7abe8a
commit 218ba75bbb
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/APM_Control/AR_PosControl.cpp
View File

@ -141,9 +141,11 @@ void AR_PosControl::update(float dt)
} }
// calculation velocity error // calculation velocity error
bool stopping = false;
if (_vel_desired_valid) { if (_vel_desired_valid) {
// add target velocity to desired velocity from position error // add target velocity to desired velocity from position error
_vel_target += _vel_desired; _vel_target += _vel_desired;
stopping = _vel_desired.is_zero();
} }
// limit velocity to maximum speed // limit velocity to maximum speed
@ -192,19 +194,21 @@ void AR_PosControl::update(float dt)
const Vector2f vel_target_FR = AP::ahrs().earth_to_body2D(_vel_target); const Vector2f vel_target_FR = AP::ahrs().earth_to_body2D(_vel_target);
// desired speed is normally the forward component (only) of the target velocity // desired speed is normally the forward component (only) of the target velocity
// but we do not let it fall below the minimum turn speed unless the vehicle is slowing down float des_speed = vel_target_FR.x;
if (!stopping) {
// do not let target speed fall below the minimum turn speed unless the vehicle is slowing down
const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min); const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min);
float des_speed;
if (_reversed != backing_up) { if (_reversed != backing_up) {
// if reversed or backing up desired speed will be negative // if reversed or backing up desired speed will be negative
des_speed = MIN(-abs_des_speed_min, vel_target_FR.x); des_speed = MIN(-abs_des_speed_min, vel_target_FR.x);
} else { } else {
des_speed = MAX(abs_des_speed_min, vel_target_FR.x); des_speed = MAX(abs_des_speed_min, vel_target_FR.x);
} }
}
_desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt); _desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt);
// calculate turn rate from desired lateral acceleration // calculate turn rate from desired lateral acceleration
_desired_lat_accel = accel_target_FR.y; _desired_lat_accel = stopping ? 0 : accel_target_FR.y;
_desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed); _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed);
} }