Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Control: add expo calculation.

Browse Source
This commit is contained in:
Leonard Hall 2021-09-17 15:22:58 +09:30 committed by Randy Mackay
parent 1d7d5d23f7
commit 77fbcf1741
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Math/control.cpp
View File

@ -407,3 +407,15 @@ float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float m
} }
return fabsf(max_z_neg/direction.z); return fabsf(max_z_neg/direction.z);
} }
// calculate the expo function on the normalised input
// input must be in the range of -1 to 1
// expo should be less than 1.0 but limited to be less than 0.95
float input_expo(float input, float expo)
{
input = constrain_float(input, -1.0, 1.0);
if (expo < 0.95) {
return (1 - expo) * input / (1 - expo * fabsf(input));
}
return input;
}

5
libraries/AP_Math/control.h
View File

@ -115,3 +115,8 @@ float stopping_distance(float velocity, float p, float accel_max);
// calculate the maximum acceleration or velocity in a given direction // calculate the maximum acceleration or velocity in a given direction
// based on horizontal and vertical limits. // based on horizontal and vertical limits.
float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg); float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float max_z_neg);
// calculate the expo function on the normalised input
// input must be in the range of -1 to 1
// expo should be less than 1.0 but limited to be less than 0.95
float input_expo(float input, float expo);