Merge pull request #1843 from martinbudden/bf_minor_optimisations

Some minor performance optimisations

src/main/common/filter.c

@@ -46,11 +46,12 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
 {
     filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
     filter->dT = dT;
+    filter->k = filter->dT / (filter->RC + filter->dT);
 }
 
 float pt1FilterApply(pt1Filter_t *filter, float input)
 {
-    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
+    filter->state = filter->state + filter->k * (input - filter->state);
     return filter->state;
 }
 
@@ -60,9 +61,10 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
     if (!filter->RC) {
         filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
         filter->dT = dT;
+        filter->k = filter->dT / (filter->RC + filter->dT);
     }
 
-    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
+    filter->state = filter->state + filter->k * (input - filter->state);
 
     return filter->state;
 }

src/main/common/filter.h

@@ -25,6 +25,7 @@
 
 typedef struct pt1Filter_s {
     float state;
+    float k;
     float RC;
     float dT;
 } pt1Filter_t;

src/main/common/maths.h

@@ -20,6 +20,7 @@
 #ifndef sq
 #define sq(x) ((x)*(x))
 #endif
+#define power3(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

src/main/fc/mw.c

@@ -147,7 +147,6 @@ bool isCalibrating()
 }
 
 #define RC_RATE_INCREMENTAL 14.54f
-#define RC_EXPO_POWER 3
 
 void calculateSetpointRate(int axis, int16_t rc) {
     float angleRate, rcRate, rcSuperfactor, rcCommandf;
@@ -167,7 +166,7 @@ void calculateSetpointRate(int axis, int16_t rc) {
 
     if (rcExpo) {
         float expof = rcExpo / 100.0f;
-        rcCommandf = rcCommandf * powerf(rcInput[axis], RC_EXPO_POWER) * expof + rcCommandf * (1-expof);
+        rcCommandf = rcCommandf * power3(rcInput[axis]) * expof + rcCommandf * (1-expof);
     }
 
     angleRate = 200.0f * rcRate * rcCommandf;
@@ -290,7 +289,7 @@ void updateRcCommands(void)
                 tmp = 0;
             }
             rcCommand[axis] = tmp;
-        } else if (axis == YAW) {
+        } else {
             if (tmp > rcControlsConfig()->yaw_deadband) {
                 tmp -= rcControlsConfig()->yaw_deadband;
             } else {
@@ -778,9 +777,11 @@ void subTaskMotorUpdate(void)
 
 #ifdef USE_SERVOS
     // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
-    servoTable();
+    if (isMixerUsingServos()) {
-    filterServos();
+        servoTable();
-    writeServos();
+        filterServos();
+        writeServos();
+    }
 #endif
 
     if (motorControlEnable) {

src/main/flight/pid.c

@@ -263,10 +263,10 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
                 dynC = c[axis];
                 if (setpointRate[axis] > 0) {
                     if ((setpointRate[axis] - previousSetpoint[axis]) < previousSetpoint[axis])
-                        dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
+                        dynC = dynC * sq(rcInput[axis]) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
                 } else if (setpointRate[axis] < 0) {
                     if ((setpointRate[axis] - previousSetpoint[axis]) > previousSetpoint[axis])
-                        dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
+                        dynC = dynC * sq(rcInput[axis]) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
                 }
             }
             const float rD = dynC * setpointRate[axis] - PVRate;    // cr - y

src/main/scheduler/scheduler.c

@@ -176,9 +176,10 @@ void setTaskEnabled(cfTaskId_e taskId, bool enabled)
 
 uint32_t getTaskDeltaTime(cfTaskId_e taskId)
 {
-    if (taskId == TASK_SELF || taskId < TASK_COUNT) {
+    if (taskId == TASK_SELF) {
-        cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
+        return currentTask->taskLatestDeltaTime;
-        return task->taskLatestDeltaTime;
+    } else if (taskId < TASK_COUNT) {
+        return cfTasks[taskId].taskLatestDeltaTime;
     } else {
         return 0;
     }