From 9a8a31676b8612e924964f212da1a5c1e63f5c20 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Wed, 10 Jun 2015 13:30:26 +0100
Subject: [PATCH] Reduce maximum servos from 10 to 8.

1) the 2 extra servos were not used for anything, yet.
2) the MSP packet to set ALL the servo configurations in one go is
larger than the MSP input buffer size.

Likely the MSP_SET_SERVO_CONF should be updated with command that takes
an index of a servo to configure, as per mode ranges/colors/leds/etc.

Fixes #1002
---
 src/main/config/config.c       |  4 ++--
 src/main/drivers/pwm_mapping.h |  4 ++--
 src/main/flight/mixer.h        |  2 +-
 src/main/io/serial_msp.c       | 34 +++++++++++++++++++++++-----------
 4 files changed, 28 insertions(+), 16 deletions(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 08c84396da..72e2873865 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -128,7 +128,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 101;
+static const uint8_t EEPROM_CONF_VERSION = 102;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -348,7 +348,7 @@ static void resetConf(void)
 {
     int i;
 #ifdef USE_SERVOS
-    int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100, 100, 100 };
+    int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
     ;
 #endif
 
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index 4e11e88f0e..dd9c89889d 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -18,10 +18,10 @@
 #pragma once
 
 #define MAX_PWM_MOTORS  12
-#define MAX_PWM_SERVOS  10
+#define MAX_PWM_SERVOS  8
 
 #define MAX_MOTORS  12
-#define MAX_SERVOS  10
+#define MAX_SERVOS  8
 #define MAX_PWM_OUTPUT_PORTS MAX_PWM_MOTORS // must be set to the largest of either MAX_MOTORS or MAX_SERVOS
 
 #if MAX_PWM_OUTPUT_PORTS < MAX_MOTORS || MAX_PWM_OUTPUT_PORTS < MAX_SERVOS
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index eb4195ad04..a297f606fe 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -18,7 +18,7 @@
 #pragma once
 
 #define MAX_SUPPORTED_MOTORS 12
-#define MAX_SUPPORTED_SERVOS 10
+#define MAX_SUPPORTED_SERVOS 8
 #define YAW_JUMP_PREVENTION_LIMIT_LOW 80
 #define YAW_JUMP_PREVENTION_LIMIT_HIGH 500
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index bf92ea1e6a..48577f1ca8 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -308,6 +308,8 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 
 #define INBUF_SIZE 64
 
+#define SERVO_CHUNK_SIZE 7
+
 typedef struct box_e {
     const uint8_t boxId;         // see boxId_e
     const char *boxName;            // GUI-readable box name
@@ -619,6 +621,8 @@ void mspReleasePortIfAllocated(serialPort_t *serialPort)
 
 void mspInit(serialConfig_t *serialConfig)
 {
+    BUILD_BUG_ON((SERVO_CHUNK_SIZE * MAX_SUPPORTED_SERVOS) > INBUF_SIZE);
+
     // calculate used boxes based on features and fill availableBoxes[] array
     memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
 
@@ -1408,18 +1412,26 @@ static bool processInCommand(void)
         break;
     case MSP_SET_SERVO_CONF:
 #ifdef USE_SERVOS
-        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-            currentProfile->servoConf[i].min = read16();
-            currentProfile->servoConf[i].max = read16();
-            // provide temporary support for old clients that try and send a channel index instead of a servo middle
-            uint16_t potentialServoMiddleOrChannelToForward = read16();
-            if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) {
-                currentProfile->servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
+        if (currentPort->dataSize % SERVO_CHUNK_SIZE != 0) {
+            debug[0] = currentPort->dataSize;
+            headSerialError(0);
+        } else {
+            uint8_t servoCount = currentPort->dataSize / SERVO_CHUNK_SIZE;
+
+            for (i = 0; i < MAX_SUPPORTED_SERVOS && i < servoCount; i++) {
+                currentProfile->servoConf[i].min = read16();
+                currentProfile->servoConf[i].max = read16();
+
+                // provide temporary support for old clients that try and send a channel index instead of a servo middle
+                uint16_t potentialServoMiddleOrChannelToForward = read16();
+                if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) {
+                    currentProfile->servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward;
+                }
+                if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) {
+                    currentProfile->servoConf[i].middle = potentialServoMiddleOrChannelToForward;
+                }
+                currentProfile->servoConf[i].rate = read8();
             }
-            if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) {
-                currentProfile->servoConf[i].middle = potentialServoMiddleOrChannelToForward;
-            }
-            currentProfile->servoConf[i].rate = read8();
         }
 #endif
         break;