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

AP_Math: minor control format fix

Browse Source
This commit is contained in:
Randy Mackay 2022-02-08 17:01:03 +09:00
parent a47fc65511
commit f57a081ceb
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Math/control.cpp
View File

@ -167,10 +167,10 @@ void shape_vel_accel(float vel_input, float accel_input,
float vel_error = vel_input - vel;
// Calculate time constants and limits to ensure stable operation
float KPa;
// The direction of acceleration limit is the same as the velocity error.
// This is because the velocity error is negative when slowing down while
// closing a positive position error.
float KPa;
if (is_positive(vel_error)) {
KPa = jerk_max / accel_max;
} else {
@ -277,12 +277,12 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
// position error to be corrected
float pos_error = pos_input - pos;
float accel_tc_max;
float KPv;
// Calculate time constants and limits to ensure stable operation
// The negative acceleration limit is used here because the square root controller
// manages the approach to the setpoint. Therefore the acceleration is in the opposite
// direction to the position error.
float accel_tc_max;
float KPv;
if (is_positive(pos_error)) {
accel_tc_max = -0.5 * accel_min;
KPv = 0.5 * jerk_max / (-accel_min);