Merge remote-tracking branch 'refs/remotes/upstream/master' into feature-timer

src/main/io/serial.c

@@ -513,6 +513,17 @@ bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t fun
     return result->portFunction->scenario & functionMask;
 }
 
+serialPort_t *findSharedSerialPort(serialPortFunction_e functionToUse, uint16_t functionMask)
+{
+    functionConstraint_t *functionConstraint = getConfiguredFunctionConstraint(functionToUse);
+    serialPortSearchResult_t *result = findSerialPort(functionToUse, functionConstraint);
+
+    if (result->portFunction->scenario & functionMask) {
+        return result->portFunction->port;
+    }
+    return NULL;
+}
+
 void applySerialConfigToPortFunctions(serialConfig_t *serialConfig)
 {
     uint32_t portIndex = 0, serialPortIdentifier;

src/main/io/serial.h

@@ -161,6 +161,7 @@ void applySerialConfigToPortFunctions(serialConfig_t *serialConfig);
 bool isSerialConfigValid(serialConfig_t *serialConfig);
 bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier);
 bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask);
+serialPort_t *findSharedSerialPort(serialPortFunction_e functionToUse, uint16_t functionMask);
 
 const serialPortFunctionList_t *getSerialPortFunctionList(void);
 

src/main/io/serial_cli.c

@@ -90,6 +90,7 @@ static void cliMixer(char *cmdline);
 static void cliMotor(char *cmdline);
 static void cliProfile(char *cmdline);
 static void cliRateProfile(char *cmdline);
+static void cliReboot(void);
 static void cliSave(char *cmdline);
 static void cliSet(char *cmdline);
 static void cliGet(char *cmdline);
@@ -332,7 +333,7 @@ const clivalue_t valueTable[] = {
 
     { "gimbal_flags",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
 
-    { "acc_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.acc_hardware, 0, 5 },
+    { "acc_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.acc_hardware, 0, ACC_NONE },
     { "acc_lpf_factor",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 },
     { "accxy_deadband",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 },
     { "accz_deadband",              VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 },
@@ -858,18 +859,14 @@ static void cliEnter(void)
 
 static void cliExit(char *cmdline)
 {
-    cliPrint("\r\nLeaving CLI mode\r\n");
+    UNUSED(cmdline);
+    cliPrint("\r\nLeaving CLI mode, unsaved changes lost.\r\n");
     *cliBuffer = '\0';
     bufferIndex = 0;
     cliMode = 0;
     // incase a motor was left running during motortest, clear it here
     mixerResetMotors();
-    // save and reboot... I think this makes the most sense - otherwise config changes can be out of sync, maybe just need to applyConfig and return?
+    cliReboot();
-#if 1
-    cliSave(cmdline);
-#else
-    releaseSerialPort(cliPort, FUNCTION_CLI);
-#endif
 }
 
 static void cliFeature(char *cmdline)

src/main/io/serial_msp.c

@@ -525,12 +525,17 @@ static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, ms
 // This rate is chosen since softserial supports it.
 #define MSP_FALLBACK_BAUDRATE 19200
 
-static void openAllMSPSerialPorts(serialConfig_t *serialConfig)
+void mspAllocateSerialPorts(serialConfig_t *serialConfig)
 {
     serialPort_t *port;
 
-    uint8_t portIndex = 0;
+    uint8_t portIndex;
-    do {
+
+    for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+        mspPort_t *mspPort = &mspPorts[portIndex];
+        if (mspPort->mspPortUsage != UNUSED_PORT) {
+            continue;
+        }
 
         uint32_t baudRate = serialConfig->msp_baudrate;
 
@@ -549,18 +554,30 @@ static void openAllMSPSerialPorts(serialConfig_t *serialConfig)
         } while (!port);
 
         if (port && portIndex < MAX_MSP_PORT_COUNT) {
-            mspPort_t *newMspPort = &mspPorts[portIndex++];
+            resetMspPort(mspPort, port, FOR_GENERAL_MSP);
-
+        }
-            resetMspPort(newMspPort, port, FOR_GENERAL_MSP);
+        if (!port) {
+            break;
+        }
     }
-
-    } while (port);
 
     // XXX this function might help with adding support for MSP on more than one port, if not delete it.
     const serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
     UNUSED(serialPortFunctionList);
 }
 
+void mspReleasePortIfAllocated(serialPort_t *serialPort)
+{
+    uint8_t portIndex;
+    for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+        mspPort_t *candidateMspPort = &mspPorts[portIndex];
+        if (candidateMspPort->port == serialPort) {
+            endSerialPortFunction(serialPort, FUNCTION_MSP);
+            memset(candidateMspPort, 0, sizeof(mspPort_t));
+        }
+    }
+}
+
 void mspInit(serialConfig_t *serialConfig)
 {
     // calculate used boxes based on features and fill availableBoxes[] array
@@ -615,14 +632,8 @@ void mspInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXSONAR;
     }
 
-    mspReset(serialConfig);
-}
-
-void mspReset(serialConfig_t *serialConfig)
-{
     memset(mspPorts, 0x00, sizeof(mspPorts));
-
+    mspAllocateSerialPorts(serialConfig);
-    openAllMSPSerialPorts(serialConfig);
 }
 
 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)

src/main/io/serial_msp.h

@@ -23,4 +23,5 @@
 void mspProcess(void);
 void sendMspTelemetry(void);
 void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);
-void mspReset(serialConfig_t *serialConfig);
+void mspAllocateSerialPorts(serialConfig_t *serialConfig);
+void mspReleasePortIfAllocated(serialPort_t *serialPort);

src/main/mw.c

@@ -299,10 +299,12 @@ void mwDisarm(void)
         DISABLE_ARMING_FLAG(ARMED);
 
 #ifdef TELEMETRY
+        if (feature(FEATURE_TELEMETRY)) {
             // the telemetry state must be checked immediately so that shared serial ports are released.
             checkTelemetryState();
             if (isSerialPortFunctionShared(FUNCTION_TELEMETRY, FUNCTION_MSP)) {
-            mspReset(&masterConfig.serialConfig);
+                mspAllocateSerialPorts(&masterConfig.serialConfig);
+            }
         }
 #endif
     }
@@ -317,6 +319,15 @@ void mwArm(void)
         if (!ARMING_FLAG(PREVENT_ARMING)) {
             ENABLE_ARMING_FLAG(ARMED);
             headFreeModeHold = heading;
+
+#ifdef TELEMETRY
+            if (feature(FEATURE_TELEMETRY)) {
+                serialPort_t *sharedTelemetryAndMspPort = findSharedSerialPort(FUNCTION_TELEMETRY, FUNCTION_MSP);
+                if (sharedTelemetryAndMspPort) {
+                    mspReleasePortIfAllocated(sharedTelemetryAndMspPort);
+                }
+            }
+#endif
             return;
         }
     }

src/main/rx/sbus.c

@@ -35,10 +35,9 @@
 #include "rx/rx.h"
 #include "rx/sbus.h"
 
-#define SBUS_MAX_CHANNEL 12
+#define SBUS_MAX_CHANNEL 16
 #define SBUS_FRAME_SIZE 25
 #define SBUS_SYNCBYTE 0x0F
-#define SBUS_OFFSET 988
 
 #define SBUS_BAUDRATE 100000
 
@@ -64,7 +63,7 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
     sBusPort = openSerialPort(FUNCTION_SERIAL_RX, sbusDataReceive, SBUS_BAUDRATE, (portMode_t)(MODE_RX | MODE_SBUS), SERIAL_INVERTED);
 
     for (b = 0; b < SBUS_MAX_CHANNEL; b++)
-        sbusChannelData[b] = 2 * (rxConfig->midrc - SBUS_OFFSET);
+        sbusChannelData[b] = (1.6f * rxConfig->midrc) - 1408;
     if (callback)
         *callback = sbusReadRawRC;
     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
@@ -85,6 +84,10 @@ struct sbus_dat {
     unsigned int chan9 : 11;
     unsigned int chan10 : 11;
     unsigned int chan11 : 11;
+    unsigned int chan12 : 11;
+    unsigned int chan13 : 11;
+    unsigned int chan14 : 11;
+    unsigned int chan15 : 11;
 } __attribute__ ((__packed__));
 
 typedef union {
@@ -127,7 +130,7 @@ bool sbusFrameComplete(void)
         return false;
     }
     sbusFrameDone = false;
-    if ((sbus.in[22] >> 3) & 0x0001) {
+    if ((sbus.in[SBUS_FRAME_SIZE - 3] >> 3) & 0x0001) {
         // internal failsafe enabled and rx failsafe flag set
         return false;
     }
@@ -143,12 +146,17 @@ bool sbusFrameComplete(void)
     sbusChannelData[9] = sbus.msg.chan9;
     sbusChannelData[10] = sbus.msg.chan10;
     sbusChannelData[11] = sbus.msg.chan11;
+    sbusChannelData[12] = sbus.msg.chan12;
+    sbusChannelData[13] = sbus.msg.chan13;
+    sbusChannelData[14] = sbus.msg.chan14;
+    sbusChannelData[15] = sbus.msg.chan15;
     return true;
 }
 
 static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 {
     UNUSED(rxRuntimeConfig);
-    return sbusChannelData[chan] / 2 + SBUS_OFFSET;
+    // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
+    // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
+    return (0.625f * sbusChannelData[chan]) + 880;
 }
-