Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00
Code Issues Wiki Activity

Fix MSP/Telemetry Arm/Disarm problems.

Browse source 
Tested with two MSP ports, one via softserial. Both with telemetry
enabled and without telemetry enabled.
This commit is contained in:
Dominic Clifton 2014-11-08 11:53:01 +00:00
parent f42d603b12
commit 28da97f894
3 changed files with 17 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

31
src/main/io/serial_msp.c
View file

@ -525,12 +525,17 @@ static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, ms
// This rate is chosen since softserial supports it. // This rate is chosen since softserial supports it.
#define MSP_FALLBACK_BAUDRATE 19200 #define MSP_FALLBACK_BAUDRATE 19200
static void openAllMSPSerialPorts(serialConfig_t *serialConfig) void mspAllocateSerialPorts(serialConfig_t *serialConfig)
{ {
serialPort_t *port; 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; uint32_t baudRate = serialConfig->msp_baudrate;
@ -549,12 +554,12 @@ static void openAllMSPSerialPorts(serialConfig_t *serialConfig)
} while (!port); } while (!port);
if (port && portIndex < MAX_MSP_PORT_COUNT) { 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) {
} while (port); break;
}
}
// XXX this function might help with adding support for MSP on more than one port, if not delete it. // XXX this function might help with adding support for MSP on more than one port, if not delete it.
const serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList(); const serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
@ -565,7 +570,7 @@ void mspReleasePortIfAllocated(serialPort_t *serialPort)
{ {
uint8_t portIndex; uint8_t portIndex;
for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
mspPort_t *candidateMspPort = &mspPorts[portIndex++]; mspPort_t *candidateMspPort = &mspPorts[portIndex];
if (candidateMspPort->port == serialPort) { if (candidateMspPort->port == serialPort) {
endSerialPortFunction(serialPort, FUNCTION_MSP); endSerialPortFunction(serialPort, FUNCTION_MSP);
memset(candidateMspPort, 0, sizeof(mspPort_t)); memset(candidateMspPort, 0, sizeof(mspPort_t));
@ -627,14 +632,8 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXSONAR; activeBoxIds[activeBoxIdCount++] = BOXSONAR;
} }
mspReset(serialConfig);
}
void mspReset(serialConfig_t *serialConfig)
{
memset(mspPorts, 0x00, sizeof(mspPorts)); memset(mspPorts, 0x00, sizeof(mspPorts));
mspAllocateSerialPorts(serialConfig);
openAllMSPSerialPorts(serialConfig);
} }
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1) #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)

2
src/main/io/serial_msp.h
View file

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

2
src/main/mw.c
View file

@ -303,7 +303,7 @@ void mwDisarm(void)
// the telemetry state must be checked immediately so that shared serial ports are released. // the telemetry state must be checked immediately so that shared serial ports are released.
checkTelemetryState(); checkTelemetryState();
if (isSerialPortFunctionShared(FUNCTION_TELEMETRY, FUNCTION_MSP)) { if (isSerialPortFunctionShared(FUNCTION_TELEMETRY, FUNCTION_MSP)) {
mspReset(&masterConfig.serialConfig); mspAllocateSerialPorts(&masterConfig.serialConfig);
} }
} }
#endif #endif