From 72cb29eca26cc7616445b0fe873dac0e80232279 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 28 Oct 2017 22:25:38 +0200
Subject: [PATCH] Speed optimization for S.Bus encoding

---
 crossbow.ino |  4 ++--
 sbus.cpp     | 11 ++++++++++-
 2 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/crossbow.ino b/crossbow.ino
index 07e3bf3..f3a6686 100644
--- a/crossbow.ino
+++ b/crossbow.ino
@@ -1,5 +1,5 @@
-#define DEVICE_MODE_TX
-// #define DEVICE_MODE_RX
+// #define DEVICE_MODE_TX
+#define DEVICE_MODE_RX
 
 #define FEATURE_TX_OLED
 
diff --git a/sbus.cpp b/sbus.cpp
index 53ffe0c..b31eb5c 100644
--- a/sbus.cpp
+++ b/sbus.cpp
@@ -12,6 +12,15 @@
 #define SBUS_STATE_FAILSAFE 0x08
 #define SBUS_STATE_SIGNALLOSS 0x04
 
+/*
+Precomputed mapping from 990-2010 to 173:1811
+equivalent to 
+map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
+*/
+int mapChannelToSbus(int in) {
+    return (((long) in * 1605l) / 1000l) - 1417;
+}
+
 void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){
     
     static int output[SBUS_CHANNEL_NUMBER] = {0};
@@ -21,7 +30,7 @@ void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool
      * 173-1811 with middle at 992 S.BUS protocol requires
      */
     for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
-        output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
+        output[i] = mapsChannelToSbus(channels[i]);
     }
 
     uint8_t stateByte = 0x00;