rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16: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]); 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 */ #endif /* MATHMISC_H */

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

@ -29,6 +29,7 @@
*/ */
#include "inc/manualcontrol.h" #include "inc/manualcontrol.h"
#include <mathmisc.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
#include <stabilizationdesired.h> #include <stabilizationdesired.h>
#include <flightmodesettings.h> #include <flightmodesettings.h>
@ -44,7 +45,8 @@ static float applyExpo(float value, float expo);
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 // magic number scales expo
// so that // so that
@ -53,9 +55,11 @@ static float applyExpo(float value, float expo)
// expo=-100 yields value**(1/10) // expo=-100 yields value**(1/10)
// (pow(10,1/100)~=1.023293) // (pow(10,1/100)~=1.023293)
if (value > 0.0f) { 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 { } else {
return -powf(-value, exp); return 0.0f;
} }
} }