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

Merge pull request #11126 from mathiasvr/pr-int-exp

Browse source 
Use macros for power with integer exponents
This commit is contained in:
J Blackman 2022-06-28 15:28:35 +10:00 • committed by GitHub
commit 00cd0e52e6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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)) #define sq(x) ((x)*(x))
#endif #endif
#define power3(x) ((x)*(x)*(x)) #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 // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
#define FAST_MATH // order 9 approximation #define FAST_MATH // order 9 approximation

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

@ -208,7 +208,7 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs
float applyActualRates(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; 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 centerSensitivity = currentControlRateProfile->rcRates[axis] * 10.0f;
const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity); const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity);

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

@ -338,7 +338,7 @@ float pidCompensateThrustLinearization(float throttle)
if (pidRuntime.thrustLinearization != 0.0f) { if (pidRuntime.thrustLinearization != 0.0f) {
// for whoops where a lot of TL is needed, allow more throttle boost // for whoops where a lot of TL is needed, allow more throttle boost
const float throttleReversed = (1.0f - throttle); const float throttleReversed = (1.0f - throttle);
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powf(throttleReversed, 2); throttle /= 1.0f + pidRuntime.throttleCompensateAmount * sq(throttleReversed);
} }
return throttle; return throttle;
} }
@ -348,7 +348,7 @@ float pidApplyThrustLinearization(float motorOutput)
if (pidRuntime.thrustLinearization != 0.0f) { if (pidRuntime.thrustLinearization != 0.0f) {
if (motorOutput > 0.0f) { if (motorOutput > 0.0f) {
const float motorOutputReversed = (1.0f - motorOutput); const float motorOutputReversed = (1.0f - motorOutput);
motorOutput *= 1.0f + powf(motorOutputReversed, 2) * pidRuntime.thrustLinearization; motorOutput *= 1.0f + sq(motorOutputReversed) * pidRuntime.thrustLinearization;
} }
} }
return motorOutput; 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 #ifdef USE_THRUST_LINEARIZATION
pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f; 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 #endif
#if defined(USE_D_MIN) #if defined(USE_D_MIN)

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

@ -1787,7 +1787,7 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
int32_t dir; int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &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) { 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; GPS_distanceFlownInCm += dist;
} }