From 76ea24e62cde944f1e096b5edd9eca25c5558f92 Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@8db61ce9-5e8f-94c8-fa6b-1d1c8bcca7d0>
Date: Tue, 21 Aug 2012 11:45:04 +0000
Subject: [PATCH] Better correction for issue 95

---
 src/drivers.cpp |  2 +-
 src/frsky.cpp   | 13 +++++++++----
 src/frsky.h     |  4 ++--
 src/open9x.cpp  | 39 ++++++++++++++++++++++++---------------
 src/open9x.h    | 24 ++++++++++++------------
 5 files changed, 48 insertions(+), 34 deletions(-)

diff --git a/src/drivers.cpp b/src/drivers.cpp
index dd4bd30e8..2cbf78297 100644
--- a/src/drivers.cpp
+++ b/src/drivers.cpp
@@ -175,7 +175,7 @@ void per10ms()
   check_mavlink();
 #endif
 
-#if defined (FRSKY) && !defined(PCBARM)
+#if defined (FRSKY) && !defined(PCBARM) && !defined(AUDIO) && !defined(VOICE)
   check_frsky();
 #endif
 
diff --git a/src/frsky.cpp b/src/frsky.cpp
index a30e67b77..87b8f9d6f 100644
--- a/src/frsky.cpp
+++ b/src/frsky.cpp
@@ -135,9 +135,14 @@ typedef enum {
   TS_XOR = 0x80 // decode stuffed byte
 } TS_STATE;
 
-int16_t applyChannelRatio(uint8_t channel, int16_t val)
+uint16_t getChannelRatio(uint8_t channel)
 {
-  return (int16_t)(((int32_t)val+g_model.frsky.channels[channel].offset) * (g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier) * 2 / 51);
+  return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier;
+}
+
+int16_t applyChannelRatio(uint8_t channel, uint8_t val)
+{
+  return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51;
 }
 
 void evalVario(int16_t altitude_bp, uint16_t altitude_ap)
@@ -660,7 +665,7 @@ inline void FRSKY10mspoll(void)
 }
 #endif
 
-void check_frsky()
+NOINLINE void check_frsky()
 {
 #if defined(PCBARM)
   rxPdcUsart(processSerialData);              // Receive serial data here
@@ -874,7 +879,7 @@ void frskyEvalCurrentConsumptionBoundary()
   currentConsumptionBoundary = 3600;
   uint8_t channel = g_model.frsky.currentSource-FRSKY_SOURCE_A1;
   if (channel <= 1) {
-    uint16_t divider = (g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier);
+    uint16_t divider = getChannelRatio(channel);
     if (divider > 5) {
       currentConsumptionBoundary = 360000L / divider;
     }
diff --git a/src/frsky.h b/src/frsky.h
index f1ccb3880..3977d6b6f 100644
--- a/src/frsky.h
+++ b/src/frsky.h
@@ -197,7 +197,7 @@ extern uint8_t frskyTxBufferCount;
 
 
 void FRSKY_Init(void);
-void check_frsky(void);
+NOINLINE void check_frsky(void);
 
 inline void FRSKY_setModelAlarms(void)
 {
@@ -215,7 +215,7 @@ int16_t convertTelemValue(uint8_t channel, uint8_t value);
 NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm);
 
 extern const pm_uint8_t bchunit_ar[];
-int16_t applyChannelRatio(uint8_t channel, int16_t val);
+int16_t applyChannelRatio(uint8_t channel, uint8_t val);
 void putsTelemetryChannel(uint8_t x, uint8_t y, uint8_t channel, int16_t val, uint8_t att);
 
 #endif
diff --git a/src/open9x.cpp b/src/open9x.cpp
index 0e02a6506..563c21da8 100644
--- a/src/open9x.cpp
+++ b/src/open9x.cpp
@@ -468,17 +468,18 @@ int16_t applyLimits(uint8_t channel, int32_t value)
 
 #if defined(PPM_LIMITS_SYMETRICAL)
   if (value) {
-    // TODO flash saving
+    int32_t tmp;
     if (g_model.limitData[channel].symetrical)
-      value = (value > 0) ? value * ((int32_t) lim_p) / 100000 :
-                           -value * ((int32_t) lim_n) / 100000; //div by 100000 -> output = -1024..1024
+      tmp = (value > 0) ? ((int32_t) lim_p) : ((int32_t) -lim_n);
     else
-      value = (value > 0) ? value * ((int32_t) lim_p - ofs) / 100000 :
-                           -value * ((int32_t) lim_n - ofs) / 100000; //div by 100000 -> output = -1024..1024
+      tmp = (value > 0) ? ((int32_t) lim_p - ofs) : ((int32_t) -lim_n + ofs);
+    value = (value * tmp) / 100000;
   }
 #else
-  if (value) value = (value > 0) ? value * ((int32_t) lim_p - ofs) / 100000 :
-                                  -value * ((int32_t) lim_n - ofs) / 100000; //div by 100000 -> output = -1024..1024
+  if (value) {
+    int32_t tmp = (value > 0) ? ((int32_t) lim_p - ofs) : ((int32_t) -lim_n + ofs);
+    value = (value * tmp) / 100000; //div by 100000 -> output = -1024..1024
+  }
 #endif
 
   value += calc1000toRESX(ofs);
@@ -2751,22 +2752,24 @@ ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer
   cli();
   
 #if defined(PCBV4)
-  static uint8_t accuracyWarble = 4; // because 16M / 1024 / 100 = 156.25. So bump every 4.
-  uint8_t bump = (!(accuracyWarble++ & 0x03)) ? 157 : 156;
   TIMSK2 &= ~(1<<OCIE2A); // stop reentrance
-  OCR2A += bump;
 #else
   TIMSK &= ~(1<<OCIE0); // stop reentrance
-#if defined(AUDIO) || defined(VOICE)
+#endif
+
+  sei();
+
+#if defined(PCBV4)
+  static uint8_t accuracyWarble = 4; // because 16M / 1024 / 100 = 156.25. So bump every 4.
+  uint8_t bump = (!(accuracyWarble++ & 0x03)) ? 157 : 156;
+  OCR2A += bump;
+#elif defined(AUDIO) || defined(VOICE)
   OCR0 += 2; // interrupt every 128us
 #else
   static uint8_t accuracyWarble = 4; // because 16M / 1024 / 100 = 156.25. So bump every 4.
   uint8_t bump = (!(accuracyWarble++ & 0x03)) ? 157 : 156;
   OCR0 += bump;
 #endif
-#endif
-
-  sei();
 
 #if defined(PCBSTD) && (defined(AUDIO) || defined(VOICE))
 
@@ -2779,6 +2782,13 @@ ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer
 #endif
 
   static uint8_t cnt10ms = 77; // execute 10ms code once every 78 ISRs
+
+#if defined(FRSKY)
+  if (cnt10ms == 30) {
+    check_frsky();
+  }
+#endif
+
   if (cnt10ms-- == 0) { // BEGIN { ... every 10ms ... }
     // Begin 10ms event
     cnt10ms = 77;
@@ -2800,7 +2810,6 @@ ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer
     heartbeat |= HEART_TIMER10ms; // TODO check heartbeat everywhere!
 #endif
 
-
 #if defined(PCBSTD) && (defined(AUDIO) || defined(VOICE))
   } // end 10ms event
 #endif
diff --git a/src/open9x.h b/src/open9x.h
index 8e788ae34..0d12e1b8b 100644
--- a/src/open9x.h
+++ b/src/open9x.h
@@ -544,9 +544,20 @@ enum PerOutMode {
 
 extern uint8_t s_perout_mode;
 
+#ifndef FORCEINLINE
+#define FORCEINLINE inline __attribute__ ((always_inline))
+#endif
+
+#ifndef NOINLINE
+#define NOINLINE __attribute__ ((noinline))
+#endif
+
+// Fiddle to force compiler to use a pointer
+#define FORCE_INDIRECT(ptr) __asm__ __volatile__ ("" : "=e" (ptr) : "0" (ptr))
+
 void    perOut(uint8_t tick10ms);
 void    perMain();
-void    per10ms();
+NOINLINE void    per10ms();
 
 int16_t getValue(uint8_t i);
 bool    getSwitch(int8_t swtch, bool nc);
@@ -705,17 +716,6 @@ void read_9_adc(void ) ;
 /// liefert Dimension eines Arrays
 #define DIM(arr) (sizeof((arr))/sizeof((arr)[0]))
 
-#ifndef FORCEINLINE
-#define FORCEINLINE inline __attribute__ ((always_inline))
-#endif
-
-#ifndef NOINLINE
-#define NOINLINE __attribute__ ((noinline))
-#endif
-
-// Fiddle to force compiler to use a pointer
-#define FORCE_INDIRECT(ptr) __asm__ __volatile__ ("" : "=e" (ptr) : "0" (ptr))
-
 /// liefert Betrag des Arguments
 template<class t> FORCEINLINE t abs(t a) { return a>0?a:-a; }
 /// liefert das Minimum der Argumente