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

AP_Math: Control Tools Enhancments 

AP_Math: Control Tools Enhancments
This commit is contained in:
Leonard Hall 2022-12-20 00:40:11 +10:30 committed by Andrew Tridgell
parent 95471aaa14
commit b220954043
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Math/control.cpp
View File

@ -299,7 +299,9 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
float vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt); float vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt);
// limit velocity between vel_min and vel_max // limit velocity between vel_min and vel_max
vel_target = constrain_float(vel_target, vel_min, vel_max); if (is_negative(vel_min) || is_positive(vel_max)) {
vel_target = constrain_float(vel_target, vel_min, vel_max);
}
// velocity correction with input velocity // velocity correction with input velocity
vel_target += vel_input; vel_target += vel_input;
@ -336,7 +338,9 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
Vector2f vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt); Vector2f vel_target = sqrt_controller(pos_error, KPv, accel_tc_max, dt);
// limit velocity to vel_max // limit velocity to vel_max
vel_target.limit_length(vel_max); if (is_positive(vel_max)) {
vel_target.limit_length(vel_max);
}
// velocity correction with input velocity // velocity correction with input velocity
vel_target = vel_target + vel_input; vel_target = vel_target + vel_input;