rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1092 from digitalentity/fast-trig 

Ported fast trigonometry functions over from @Crashpilot1000 Harakiri
This commit is contained in:
Dominic Clifton 2015-07-09 23:41:04 +01:00
parent 1ead7649fb a3b57386c2
commit 164fd4ec57
6 changed files with 85 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/main/common/maths.c
View File

@ -21,6 +21,44 @@
#include "axis.h"
#include "maths.h"
// http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
// Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
// https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY)
#if defined(EVEN_FASTER_TRIGONOMETRY)
#define sinApproxCoef3 -1.666568107e-1f
#define sinApproxCoef5 8.312366210e-3f
#define sinApproxCoef7 -1.849218155e-4f
#else
#define sinApproxCoef3 -1.666665710e-1f // Double: -1.666665709650470145824129400050267289858e-1
#define sinApproxCoef5 8.333017292e-3f // Double: 8.333017291562218127986291618761571373087e-3
#define sinApproxCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
#define sinApproxCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
#endif
float sin_approx(float x)
{
int32_t xint = x;
if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
while (x < -M_PIf) x += (2.0f * M_PIf);
if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
float x2 = x * x;
#if defined(EVEN_FASTER_TRIGONOMETRY)
return x + x * x2 * (sinApproxCoef3 + x2 * (sinApproxCoef5 + x2 * sinApproxCoef7));
#else
return x + x * x2 * (sinApproxCoef3 + x2 * (sinApproxCoef5 + x2 * (sinApproxCoef7 + x2 * sinApproxCoef9)));
#endif
}
float cos_approx(float x)
{
return sin_approx(x + (0.5f * M_PIf));
}
#endif
int32_t applyDeadband(int32_t value, int32_t deadband)
{
if (ABS(value) < deadband) {
@ -111,12 +149,12 @@ void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->angles.roll);
sinx = sinf(delta->angles.roll);
cosy = cosf(delta->angles.pitch);
siny = sinf(delta->angles.pitch);
cosz = cosf(delta->angles.yaw);
sinz = sinf(delta->angles.yaw);
cosx = cos_approx(delta->angles.roll);
sinx = sin_approx(delta->angles.roll);
cosy = cos_approx(delta->angles.pitch);
siny = sin_approx(delta->angles.pitch);
cosz = cos_approx(delta->angles.yaw);
sinz = sin_approx(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;

12
src/main/common/maths.h
View File

@ -21,6 +21,10 @@
#define sq(x) ((x)*(x))
#endif
// Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
#define FAST_TRIGONOMETRY // order 9 approximation
//#define EVEN_FASTER_TRIGONOMETRY // order 7 approximation
// Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f
@ -83,3 +87,11 @@ int32_t quickMedianFilter3(int32_t * v);
int32_t quickMedianFilter5(int32_t * v);
int32_t quickMedianFilter7(int32_t * v);
int32_t quickMedianFilter9(int32_t * v);
#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY)
float sin_approx(float x);
float cos_approx(float x);
#else
#define sin_approx(x) sinf(x)
#define cos_approx(x) cosf(x)
#endif

12
src/main/flight/imu.c
View File

@ -86,7 +86,7 @@ void imuConfigure(
void imuInit()
{
smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
smallAngle = lrintf(acc_1G * cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)));
accVelScale = 9.80665f / acc_1G / 10000.0f;
gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
}
@ -208,10 +208,10 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
{
int16_t head;
float cosineRoll = cosf(anglerad[AI_ROLL]);
float sineRoll = sinf(anglerad[AI_ROLL]);
float cosinePitch = cosf(anglerad[AI_PITCH]);
float sinePitch = sinf(anglerad[AI_PITCH]);
float cosineRoll = cos_approx(anglerad[AI_ROLL]);
float sineRoll = sin_approx(anglerad[AI_ROLL]);
float cosinePitch = cos_approx(anglerad[AI_PITCH]);
float sinePitch = sin_approx(anglerad[AI_PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
//TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
@ -326,5 +326,5 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f)));
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
}

12
src/main/flight/navigation.c
View File

@ -413,7 +413,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
static void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
GPS_scaleLonDown = cosf(rads);
GPS_scaleLonDown = cos_approx(rads);
}
////////////////////////////////////////////////////////////////////////////////////
@ -559,8 +559,8 @@ static void GPS_calc_nav_rate(uint16_t max_speed)
// nav_bearing includes crosstrack
temp = (9000l - nav_bearing) * RADX100;
trig[GPS_X] = cosf(temp);
trig[GPS_Y] = sinf(temp);
trig[GPS_X] = cos_approx(temp);
trig[GPS_Y] = sin_approx(temp);
for (axis = 0; axis < 2; axis++) {
rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
@ -583,7 +583,7 @@ static void GPS_update_crosstrack(void)
{
if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
crosstrack_error = sin_approx(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
} else {
@ -644,8 +644,8 @@ static int32_t wrap_36000(int32_t angle)
void updateGpsStateForHomeAndHoldMode(void)
{
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
float sin_yaw_y = sin_approx(heading * 0.0174532925f);
float cos_yaw_x = cos_approx(heading * 0.0174532925f);
if (gpsProfile->nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate);

4
src/main/mw.c
View File

@ -240,8 +240,8 @@ void annexCode(void)
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(heading - headFreeModeHold);
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;

30
src/test/unit/maths_unittest.cc
View File

@ -73,33 +73,33 @@ TEST(MathsUnittest, TestConstrainNegatives)
TEST(MathsUnittest, TestConstrainf)
{
// Within bounds.
EXPECT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f);
// Equal to bottom bound.
EXPECT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f);
// Equal to top bound.
EXPECT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f);
// Equal to both bottom and top bound.
EXPECT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f);
// Above top bound.
EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
// Below bottom bound.
EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
// Above bouth bounds.
EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f);
// Below bouth bounds.
EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f);
}
TEST(MathsUnittest, TestDegreesToRadians)
{
EXPECT_EQ(degreesToRadians(0), 0.0f);
EXPECT_EQ(degreesToRadians(90), 0.5f * M_PIf);
EXPECT_EQ(degreesToRadians(180), M_PIf);
EXPECT_EQ(degreesToRadians(-180), - M_PIf);
EXPECT_FLOAT_EQ(degreesToRadians(0), 0.0f);
EXPECT_FLOAT_EQ(degreesToRadians(90), 0.5f * M_PIf);
EXPECT_FLOAT_EQ(degreesToRadians(180), M_PIf);
EXPECT_FLOAT_EQ(degreesToRadians(-180), - M_PIf);
}
TEST(MathsUnittest, TestApplyDeadband)
@ -118,9 +118,9 @@ TEST(MathsUnittest, TestApplyDeadband)
void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b)
{
EXPECT_EQ(a->X, b->X);
EXPECT_EQ(a->Y, b->Y);
EXPECT_EQ(a->Z, b->Z);
EXPECT_FLOAT_EQ(a->X, b->X);
EXPECT_FLOAT_EQ(a->Y, b->Y);
EXPECT_FLOAT_EQ(a->Z, b->Z);
}
TEST(MathsUnittest, TestRotateVectorWithNoAngle)