diff --git a/Makefile b/Makefile
index 6d6172465f..0e781a0d47 100755
--- a/Makefile
+++ b/Makefile
@@ -271,7 +271,6 @@ HIGHEND_SRC = \
telemetry/telemetry.c \
telemetry/frsky.c \
telemetry/hott.c \
- telemetry/msp.c \
telemetry/smartport.c \
sensors/sonar.c \
sensors/barometer.c \
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 2af50b7c89..65b84b302d 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -29,7 +29,7 @@ typedef enum {
FUNCTION_GPS = (1 << 1), // 2
FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4
FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8
- FUNCTION_TELEMETRY_MSP = (1 << 4), // 16
+ FUNCTION_TELEMETRY_MSP = (1 << 4), // 16 - Note: Unused since Cleanflight 1.12.0, likely use this bitmask for flag for LTM in the future.
FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32
FUNCTION_RX_SERIAL = (1 << 6), // 64
FUNCTION_BLACKBOX = (1 << 7) // 128
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 79ae44a9c8..d7a3033f39 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -388,14 +388,8 @@ typedef enum {
COMMAND_RECEIVED
} mspState_e;
-typedef enum {
- UNUSED_PORT = 0,
- FOR_GENERAL_MSP,
- FOR_TELEMETRY
-} mspPortUsage_e;
-
typedef struct mspPort_s {
- serialPort_t *port;
+ serialPort_t *port; // null when port unused.
uint8_t offset;
uint8_t dataSize;
uint8_t checksum;
@@ -403,7 +397,6 @@ typedef struct mspPort_s {
uint8_t inBuf[INBUF_SIZE];
mspState_e c_state;
uint8_t cmdMSP;
- mspPortUsage_e mspPortUsage;
} mspPort_t;
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
@@ -584,12 +577,11 @@ static void serializeDataflashReadReply(uint32_t address, uint8_t size)
}
#endif
-static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage)
+static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
{
memset(mspPortToReset, 0, sizeof(mspPort_t));
mspPortToReset->port = serialPort;
- mspPortToReset->mspPortUsage = usage;
}
void mspAllocateSerialPorts(serialConfig_t *serialConfig)
@@ -604,14 +596,14 @@ void mspAllocateSerialPorts(serialConfig_t *serialConfig)
while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
mspPort_t *mspPort = &mspPorts[portIndex];
- if (mspPort->mspPortUsage != UNUSED_PORT) {
+ if (mspPort->port) {
portIndex++;
continue;
}
serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
if (serialPort) {
- resetMspPort(mspPort, serialPort, FOR_GENERAL_MSP);
+ resetMspPort(mspPort, serialPort);
portIndex++;
}
@@ -1886,7 +1878,7 @@ void mspProcess(void)
for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
candidatePort = &mspPorts[portIndex];
- if (candidatePort->mspPortUsage != FOR_GENERAL_MSP) {
+ if (!candidatePort->port) {
continue;
}
@@ -1915,72 +1907,3 @@ void mspProcess(void)
}
}
}
-
-static const uint8_t mspTelemetryCommandSequence[] = {
- MSP_BOXNAMES, // repeat boxnames, in case the first transmission was lost or never received.
- MSP_STATUS,
- MSP_IDENT,
- MSP_RAW_IMU,
- MSP_ALTITUDE,
- MSP_RAW_GPS,
- MSP_RC,
- MSP_MOTOR_PINS,
- MSP_ATTITUDE,
- MSP_SERVO
-};
-
-#define TELEMETRY_MSP_COMMAND_SEQUENCE_ENTRY_COUNT (sizeof(mspTelemetryCommandSequence) / sizeof(mspTelemetryCommandSequence[0]))
-
-static mspPort_t *mspTelemetryPort = NULL;
-
-void mspSetTelemetryPort(serialPort_t *serialPort)
-{
- uint8_t portIndex;
- mspPort_t *candidatePort = NULL;
- mspPort_t *matchedPort = NULL;
-
- // find existing telemetry port
- for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
- candidatePort = &mspPorts[portIndex];
- if (candidatePort->mspPortUsage == FOR_TELEMETRY) {
- matchedPort = candidatePort;
- break;
- }
- }
-
- if (!matchedPort) {
- // find unused port
- for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
- candidatePort = &mspPorts[portIndex];
- if (candidatePort->mspPortUsage == UNUSED_PORT) {
- matchedPort = candidatePort;
- break;
- }
- }
- }
- mspTelemetryPort = matchedPort;
- if (!mspTelemetryPort) {
- return;
- }
-
- resetMspPort(mspTelemetryPort, serialPort, FOR_TELEMETRY);
-}
-
-void sendMspTelemetry(void)
-{
- static uint32_t sequenceIndex = 0;
-
- if (!mspTelemetryPort) {
- return;
- }
-
- setCurrentPort(mspTelemetryPort);
-
- processOutCommand(mspTelemetryCommandSequence[sequenceIndex]);
- tailSerialReply();
-
- sequenceIndex++;
- if (sequenceIndex >= TELEMETRY_MSP_COMMAND_SEQUENCE_ENTRY_COUNT) {
- sequenceIndex = 0;
- }
-}
diff --git a/src/main/telemetry/msp.c b/src/main/telemetry/msp.c
deleted file mode 100644
index c15093d7d7..0000000000
--- a/src/main/telemetry/msp.c
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-/*
- * telemetry_MSP.c
- *
- * Created on: 22 Apr 2014
- * Author: trey marc
- */
-#include
-#include
-#include
-
-#include "platform.h"
-
-#include "build_config.h"
-
-#ifdef TELEMETRY
-
-#include "drivers/serial.h"
-#include "io/serial.h"
-#include "io/serial_msp.h"
-
-#include "telemetry/telemetry.h"
-#include "telemetry/msp.h"
-
-static telemetryConfig_t *telemetryConfig;
-static serialPortConfig_t *portConfig;
-
-static bool mspTelemetryEnabled = false;
-static portSharing_e mspPortSharing;
-
-#define TELEMETRY_MSP_INITIAL_PORT_MODE MODE_TX
-
-static serialPort_t *mspTelemetryPort = NULL;
-
-void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig)
-{
- telemetryConfig = initialTelemetryConfig;
- portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MSP);
- mspPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MSP);
-}
-
-void checkMSPTelemetryState(void)
-{
- bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mspPortSharing);
-
- if (newTelemetryEnabledValue == mspTelemetryEnabled) {
- return;
- }
-
- if (newTelemetryEnabledValue)
- configureMSPTelemetryPort();
- else
- freeMSPTelemetryPort();
-}
-
-void handleMSPTelemetry(void)
-{
- if (!mspTelemetryEnabled) {
- return;
- }
-
- if (!mspTelemetryPort) {
- return;
- }
-
- sendMspTelemetry();
-}
-
-void freeMSPTelemetryPort(void)
-{
- mspReleasePortIfAllocated(mspTelemetryPort);
- closeSerialPort(mspTelemetryPort);
- mspTelemetryPort = NULL;
- mspTelemetryEnabled = false;
-}
-
-void configureMSPTelemetryPort(void)
-{
- if (!portConfig) {
- return;
- }
-
- baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
- if (baudRateIndex == BAUD_AUTO) {
- baudRateIndex = BAUD_19200;
- }
-
- mspTelemetryPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MSP, NULL, baudRates[baudRateIndex], TELEMETRY_MSP_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
-
- if (!mspTelemetryPort) {
- return;
- }
- mspSetTelemetryPort(mspTelemetryPort);
-
- mspTelemetryEnabled = true;
-}
-
-#endif
diff --git a/src/main/telemetry/msp.h b/src/main/telemetry/msp.h
deleted file mode 100644
index eb06ae54e5..0000000000
--- a/src/main/telemetry/msp.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-/*
- * telemetry_MSP.h
- *
- * Created on: 22 Apr 2014
- * Author: trey marc
- */
-
-#ifndef TELEMETRY_MSP_H_
-#define TELEMETRY_MSP_H_
-
-void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig);
-void handleMSPTelemetry(void);
-void checkMSPTelemetryState(void);
-
-void freeMSPTelemetryPort(void);
-void configureMSPTelemetryPort(void);
-
-#endif /* TELEMETRY_MSP_H_ */
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 4edc4b6e47..18476a69e2 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -38,7 +38,6 @@
#include "telemetry/telemetry.h"
#include "telemetry/frsky.h"
#include "telemetry/hott.h"
-#include "telemetry/msp.h"
#include "telemetry/smartport.h"
static telemetryConfig_t *telemetryConfig;
@@ -52,7 +51,6 @@ void telemetryInit(void)
{
initFrSkyTelemetry(telemetryConfig);
initHoTTTelemetry(telemetryConfig);
- initMSPTelemetry(telemetryConfig);
initSmartPortTelemetry(telemetryConfig);
telemetryCheckState();
@@ -76,7 +74,6 @@ void telemetryCheckState(void)
{
checkFrSkyTelemetryState();
checkHoTTTelemetryState();
- checkMSPTelemetryState();
checkSmartPortTelemetryState();
}
@@ -84,7 +81,6 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
handleFrSkyTelemetry(rxConfig, deadband3d_throttle);
handleHoTTTelemetry();
- handleMSPTelemetry();
handleSmartPortTelemetry();
}