Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 03:50:02 +03:00
Code Issues Wiki Activity

Use macros for power with integer exponents

Browse source
This commit is contained in:
Mathias Rasmussen 2021-12-14 11:53:09 +01:00
parent adcc36888b
commit 1b95e3c20f
5 changed files with 6 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/common/maths.h
View file

@ -26,6 +26,7 @@
#define sq(x) ((x)*(x))
#endif
#define power3(x) ((x)*(x)*(x))
#define power5(x) ((x)*(x)*(x)*(x)*(x))
// Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
#define FAST_MATH // order 9 approximation

2
src/main/fc/rc.c
View file

@ -213,7 +213,7 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs
float applyActualRates(const int axis, float rcCommandf, const float rcCommandfAbs)
{
float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
expof = rcCommandfAbs * (powf(rcCommandf, 5) * expof + rcCommandf * (1 - expof));
expof = rcCommandfAbs * (power5(rcCommandf) * expof + rcCommandf * (1 - expof));
const float centerSensitivity = currentControlRateProfile->rcRates[axis] * 10.0f;
const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity);

4
src/main/flight/pid.c
View file

@ -332,7 +332,7 @@ float pidCompensateThrustLinearization(float throttle)
if (pidRuntime.thrustLinearization != 0.0f) {
// for whoops where a lot of TL is needed, allow more throttle boost
const float throttleReversed = (1.0f - throttle);
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powf(throttleReversed, 2);
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * sq(throttleReversed);
}
return throttle;
}
@ -342,7 +342,7 @@ float pidApplyThrustLinearization(float motorOutput)
if (pidRuntime.thrustLinearization != 0.0f) {
if (motorOutput > 0.0f) {
const float motorOutputReversed = (1.0f - motorOutput);
motorOutput *= 1.0f + powf(motorOutputReversed, 2) * pidRuntime.thrustLinearization;
motorOutput *= 1.0f + sq(motorOutputReversed) * pidRuntime.thrustLinearization;
}
}
return motorOutput;

2
src/main/flight/pid_init.c
View file

@ -393,7 +393,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#ifdef USE_THRUST_LINEARIZATION
pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powf(pidRuntime.thrustLinearization, 2);
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * sq(pidRuntime.thrustLinearization);
#endif
#if defined(USE_D_MIN)

2
src/main/io/gps.c
View file

@ -1511,7 +1511,7 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
if (gpsConfig()->gps_use_3d_speed) {
dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
dist = sqrtf(sq(gpsSol.llh.altCm - lastAlt) + sq(dist));
}
GPS_distanceFlownInCm += dist;
}