From 53ee863c94f7e9c04d4adb76dcaf0046d0eeb515 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphael.coeffic@frafos.com>
Date: Wed, 16 Mar 2022 16:35:55 +0100
Subject: [PATCH] Implemented proper logic for number of channels (incl. start
 channel)

---
 radio/src/gui/colorlcd/model_setup.cpp |  5 ++++-
 radio/src/gui/gui_common.h             |  9 +++++----
 radio/src/pulses/dsm2.cpp              | 20 +++++++++++---------
 radio/src/pulses/modules_helpers.h     |  5 +++++
 radio/src/telemetry/spektrum.cpp       |  2 +-
 5 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/radio/src/gui/colorlcd/model_setup.cpp b/radio/src/gui/colorlcd/model_setup.cpp
index 76e02213d8c..0f711f1d50a 100644
--- a/radio/src/gui/colorlcd/model_setup.cpp
+++ b/radio/src/gui/colorlcd/model_setup.cpp
@@ -1103,7 +1103,10 @@ class ModuleWindow : public FormGroup {
           });
           bindButton->setCheckHandler([=]() {
               if (moduleState[moduleIdx].mode != MODULE_MODE_BIND) {
-                bindButton->check(false);
+                if (bindButton->checked()) {
+                  bindButton->check(false);
+                  this->invalidate();
+                }
               }
 #if defined(MULTIMODULE)
               if (isModuleMultimodule(moduleIdx) &&
diff --git a/radio/src/gui/gui_common.h b/radio/src/gui/gui_common.h
index 22f2ae096b6..d3ae98ce31c 100644
--- a/radio/src/gui/gui_common.h
+++ b/radio/src/gui/gui_common.h
@@ -183,11 +183,12 @@ inline uint8_t MODULE_CHANNELS_ROWS(int moduleIdx)
       return 1;
     else
       return 0;
-  }
-  else if (isModuleDSM2(moduleIdx) || isModuleCrossfire(moduleIdx) || isModuleGhost(moduleIdx) || isModuleSBUS(moduleIdx)) {
+  } else if (isModuleDSM2(moduleIdx) || isModuleCrossfire(moduleIdx) ||
+             isModuleGhost(moduleIdx) || isModuleSBUS(moduleIdx) ||
+             isModuleDSMP(moduleIdx)) {
+    // fixed number of channels
     return 0;
-  }
-  else {
+  } else {
     return 1;
   }
 }
diff --git a/radio/src/pulses/dsm2.cpp b/radio/src/pulses/dsm2.cpp
index 3114258b878..6a8a58d1b30 100644
--- a/radio/src/pulses/dsm2.cpp
+++ b/radio/src/pulses/dsm2.cpp
@@ -136,9 +136,10 @@ void setupPulsesLemonDSMP()
   reset_dsm2_buffer();
 
   const auto& md = g_model.moduleData[EXTERNAL_MODULE];
-  // TODO: sanetize 'channels'
+
+  uint8_t start_channel = md.channelsStart;
   auto channels = md.getChannelsCount();
-  auto flags = md.dsmp.flags;
+  auto flags = md.dsmp.flags & 0x3F;
 
   // Force setup packet in Bind mode.
   auto module_mode = getModuleMode(EXTERNAL_MODULE);
@@ -170,35 +171,36 @@ void setupPulsesLemonDSMP()
 
   } else {
 
-    uint8_t start_channel = 0;
+    uint8_t current_channel = 0;
     if (pass == 2) {
-      start_channel += 7;
+      current_channel += 7;
     }
 
     // Send channels
     for (int i=0; i<7; i++) {
 
-      uint8_t channel = start_channel + i;
-      if (channel < channels) {
+      if (current_channel < channels) {
         
+        uint8_t channel = start_channel + current_channel;
         int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER;
         uint16_t pulse;
 
         // Use 11-bit ?
         if (flags & (1 << 2)) {
-          pulse = limit(0, ((value*349)>>9)+1024, 2047) | (channel << 11);
+          pulse = limit(0, ((value*349)>>9)+1024, 2047) | (current_channel << 11);
         } else {
-          pulse = limit(0, ((value*13)>>5)+512, 1023) | (channel << 10);
+          pulse = limit(0, ((value*13)>>5)+512, 1023) | (current_channel << 10);
         }
 
         sendByteDSMP( pulse >> 8 );
         sendByteDSMP( pulse & 0xFF );
       } else {
         // Outside of announced number of channels:
-        // -> send invalid values
+        // -> send invalid value
         sendByteDSMP( 0xFF );
         sendByteDSMP( 0xFF );
       }
+      current_channel++;
     }
   }
 
diff --git a/radio/src/pulses/modules_helpers.h b/radio/src/pulses/modules_helpers.h
index 07651c7cc8f..e04edcb5d05 100644
--- a/radio/src/pulses/modules_helpers.h
+++ b/radio/src/pulses/modules_helpers.h
@@ -477,6 +477,9 @@ inline int8_t maxModuleChannels_M8(uint8_t moduleIdx)
     return 10;
   } else if (isModuleMultimoduleDSM2(moduleIdx)) {
     return 4;  // 12 channels
+  } else if (isModuleDSMP(moduleIdx) &&
+             (g_model.moduleData[moduleIdx].dsmp.flags != 0)) {
+    return g_model.moduleData[moduleIdx].channelsCount;
   } else {
     return maxChannelsModules_M8[g_model.moduleData[moduleIdx].type];
   }
@@ -495,6 +498,8 @@ inline int8_t minModuleChannels(uint8_t idx)
     return GHOST_CHANNELS_COUNT;
   else if (isModuleSBUS(idx))
     return 16;
+  else if (isModuleDSMP(idx))
+    return maxModuleChannels(idx);
   else
     return 1;
 }
diff --git a/radio/src/telemetry/spektrum.cpp b/radio/src/telemetry/spektrum.cpp
index 8323c771696..f11503724de 100644
--- a/radio/src/telemetry/spektrum.cpp
+++ b/radio/src/telemetry/spektrum.cpp
@@ -508,7 +508,7 @@ void processDSMBindPacket(uint8_t module, const uint8_t *packet)
   if (g_model.moduleData[module].type == MODULE_TYPE_LEMON_DSMP) {
 
     // save flags
-    g_model.moduleData[module].dsmp.flags = packet[0] & 0x3F;
+    g_model.moduleData[module].dsmp.flags = packet[0];
 
     // save number of channels
     uint8_t channels = packet[2];