From 7de84b900b0954eef03067e6d943334a71f5c072 Mon Sep 17 00:00:00 2001
From: 3djc <lesitewebdejc@hotmail.com>
Date: Tue, 11 Aug 2020 09:48:19 +0200
Subject: [PATCH] CRSF ppm center handling (#7870)

* Add ppm center handling to crsf

* Improve

* Cosmetics

* Cosmetics

* Fixing scaling

* Fixing scaling

* Fixing scaling
---
 radio/src/pulses/crossfire.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp
index 5e4ffdb95..2f5427902 100644
--- a/radio/src/pulses/crossfire.cpp
+++ b/radio/src/pulses/crossfire.cpp
@@ -20,8 +20,13 @@
 
 #include "opentx.h"
 
-#define CROSSFIRE_CH_CENTER         0x3E0
 #define CROSSFIRE_CH_BITS           11
+#define CROSSFIRE_CENTER            0x3E0
+#if defined(PPM_CENTER_ADJUSTABLE)
+  #define CROSSFIRE_CENTER_CH_OFFSET(ch)            ((2 * limitAddress(ch)->ppmCenter) + 1)  // + 1 is for rouding
+#else
+  #define CROSSFIRE_CENTER_CH_OFFSET(ch)            (0)
+#endif
 
 // Range for pulses (channels output) is [-1024:+1024]
 uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses)
@@ -34,7 +39,7 @@ uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses)
   uint32_t bits = 0;
   uint8_t bitsavailable = 0;
   for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
-    uint32_t val = limit(0, CROSSFIRE_CH_CENTER + (((pulses[i]) * 4) / 5), 2*CROSSFIRE_CH_CENTER);
+    uint32_t val = limit(0, CROSSFIRE_CENTER + (CROSSFIRE_CENTER_CH_OFFSET(i) * 4) / 5 + (pulses[i] * 4) / 5, 2 * CROSSFIRE_CENTER);
     bits |= val << bitsavailable;
     bitsavailable += CROSSFIRE_CH_BITS;
     while (bitsavailable >= 8) {