Moved 'releaseSharedTelemetryPorts' to telemetry.c.

This commit is contained in:
Michael Keller 2016-12-02 19:57:03 +13:00
parent ec0b52aea1
commit ff6752cf38
3 changed files with 13 additions and 12 deletions

View File

@ -373,18 +373,6 @@ void mwDisarm(void)
}
}
#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)
#ifdef TELEMETRY
static void releaseSharedTelemetryPorts(void) {
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
while (sharedPort) {
mspSerialReleasePortIfAllocated(sharedPort);
sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
}
}
#endif
void mwArm(void)
{
static bool firstArmingCalibrationWasCompleted;

View File

@ -35,6 +35,8 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "msp/msp_serial.h"
#include "rx/rx.h"
#include "telemetry/telemetry.h"
@ -156,4 +158,13 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb
#endif
}
#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)
void releaseSharedTelemetryPorts(void) {
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
while (sharedPort) {
mspSerialReleasePortIfAllocated(sharedPort);
sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
}
}
#endif

View File

@ -60,3 +60,5 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing);
void telemetryUseConfig(telemetryConfig_t *telemetryConfig);
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
void releaseSharedTelemetryPorts(void);