rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

OP-1516 use fastPow implementation for expo - to make it work on cc3d

Browse Source
This commit is contained in:
Corvus Corax 2014-10-05 01:25:15 +02:00
parent 54575b3fc8
commit 9525398deb
2 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
flight/libraries/math/mathmisc.h
View File

@ -120,4 +120,19 @@ static inline float y_on_curve(float x, const pointf points[], int num_points)
return y_on_line(x, &points[end_point - 1], &points[end_point]);
}
/**
* Ultrafast pow() aproximation needed for expo
* Based on Algorithm by Martin Ankerl
*/
static inline float fastPow(float a, float b)
{
union {
double d;
int32_t x[2];
} u = { (double)a };
u.x[1] = (int32_t)(b * (u.x[1] - 1072632447) + 1072632447);
u.x[0] = 0;
return (float)u.d;
}
#endif /* MATHMISC_H */

10
flight/modules/ManualControl/stabilizedhandler.c
View File

@ -29,6 +29,7 @@
*/
#include "inc/manualcontrol.h"
#include <mathmisc.h>
#include <manualcontrolcommand.h>
#include <stabilizationdesired.h>
#include <flightmodesettings.h>
@ -44,7 +45,8 @@ static float applyExpo(float value, float expo);
static float applyExpo(float value, float expo)
{
float exp = powf(1.023293f, expo);
// note: fastPow makes a small error, therefore result needs to be bound
float exp = boundf(fastPow(1.023293f, expo), -10.0f, 10.0f);
// magic number scales expo
// so that
@ -53,9 +55,11 @@ static float applyExpo(float value, float expo)
// expo=-100 yields value**(1/10)
// (pow(10,1/100)~=1.023293)
if (value > 0.0f) {
return powf(value, exp);
return boundf(fastPow(value, exp), 0.0f, 1.0f);
} else if (value < -0.0f) {
return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
} else {
return -powf(-value, exp);
return 0.0f;
}
}