From 47c493b34a43c3e7f29ccbeebfaf133aa64159a1 Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Wed, 30 Dec 2015 00:03:33 +0100
Subject: [PATCH] 2khz Performance Enhancements // More processing time

---
 src/main/common/maths.c      |  4 ++--
 src/main/drivers/gyro_sync.h |  2 +-
 src/main/rx/rx.c             | 25 ++++++++++++++++++++++---
 src/main/scheduler.c         |  3 +--
 4 files changed, 26 insertions(+), 8 deletions(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index cc1599bec5..a59634db53 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -111,7 +111,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
     return value;
 }
 
-int constrain(int amt, int low, int high)
+inline int constrain(int amt, int low, int high)
 {
     if (amt < low)
         return low;
@@ -121,7 +121,7 @@ int constrain(int amt, int low, int high)
         return amt;
 }
 
-float constrainf(float amt, float low, float high)
+inline float constrainf(float amt, float low, float high)
 {
     if (amt < low)
         return low;
diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h
index 021dc63465..57d8b4485a 100644
--- a/src/main/drivers/gyro_sync.h
+++ b/src/main/drivers/gyro_sync.h
@@ -5,7 +5,7 @@
  *      Author: borisb
  */
 
-#define INTERRUPT_WAIT_TIME 10
+#define INTERRUPT_WAIT_TIME 12
 
 extern uint32_t targetLooptime;
 
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index a653bbe463..83f6df5ca2 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -41,6 +41,7 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/system.h"
+#include "drivers/gyro_sync.h"
 #include "rx/pwm.h"
 #include "rx/sbus.h"
 #include "rx/spektrum.h"
@@ -91,6 +92,11 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 #define DELAY_5_HZ (1000000 / 5)
 #define SKIP_RC_ON_SUSPEND_PERIOD 1500000           // 1.5 second period in usec (call frequency independent)
 #define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements (timing independent)
+#ifdef STM32F303xC
+#define MAX_RC_CHANNELS_HIGH_PERFORMANCE 10         // Maximum channels allowed during fast refresh rates for more performance
+#else
+#define MAX_RC_CHANNELS_HIGH_PERFORMANCE 8          // Maximum channels allowed during fast refresh rates for more performance
+#endif
 
 rxRuntimeConfig_t rxRuntimeConfig;
 static rxConfig_t *rxConfig;
@@ -439,11 +445,24 @@ STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChanne
     return sample;
 }
 
+static uint8_t getRxChannelCount(void) {
+    static uint8_t maxChannelsAllowed;
+
+    if (!maxChannelsAllowed) {
+        if (targetLooptime < 1000) {
+            maxChannelsAllowed = MAX_RC_CHANNELS_HIGH_PERFORMANCE;
+        } else {
+            maxChannelsAllowed = rxRuntimeConfig.channelCount;
+        }
+    }
+    return maxChannelsAllowed;
+}
+
 static void readRxChannelsApplyRanges(void)
 {
     uint8_t channel;
 
-    for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+    for (channel = 0; channel < getRxChannelCount(); channel++) {
 
         uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
 
@@ -484,7 +503,7 @@ static void detectAndApplySignalLossBehaviour(void)
 
     rxResetFlightChannelStatus();
 
-    for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+    for (channel = 0; channel < getRxChannelCount(); channel++) {
 
         sample = (useValueFromRx) ? rcRaw[channel] : PPM_RCVR_TIMEOUT;
 
@@ -516,7 +535,7 @@ static void detectAndApplySignalLossBehaviour(void)
         rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true;
         failsafeOnValidDataFailed();
 
-        for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+        for (channel = 0; channel < getRxChannelCount(); channel++) {
             rcData[channel] = getRxfailValue(channel);
         }
     }
diff --git a/src/main/scheduler.c b/src/main/scheduler.c
index 9d8d97506f..b23f0c05fb 100755
--- a/src/main/scheduler.c
+++ b/src/main/scheduler.c
@@ -206,7 +206,6 @@ static cfTask_t cfTasks[TASK_COUNT] = {
 
 #define REALTIME_GUARD_INTERVAL_MIN     10
 #define REALTIME_GUARD_INTERVAL_MAX     300
-#define REALTIME_GUARD_INTERVAL_MARGIN  25
 
 void taskSystem(void)
 {
@@ -227,7 +226,7 @@ void taskSystem(void)
         }
     }
 
-    realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX) + REALTIME_GUARD_INTERVAL_MARGIN;
+    realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX);
 #if defined SCHEDULER_DEBUG
     debug[2] = realtimeGuardInterval;
 #endif