From 1cbe166c497959fb917e8e3f6aef5efeab7e5d36 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 6 Apr 2014 23:07:58 +0100 Subject: [PATCH] Move common telemetry code to telemetry_common.c/.h --- Makefile | 1 + src/main.c | 2 ++ src/mw.c | 2 ++ src/mw.h | 4 --- src/serial.c | 2 ++ src/telemetry_common.c | 51 ++++++++++++++++++++++++++++++++++ src/telemetry_common.h | 17 ++++++++++++ src/telemetry_frsky.c | 62 ++++++++---------------------------------- src/telemetry_frsky.h | 14 ++++++++++ 9 files changed, 100 insertions(+), 55 deletions(-) create mode 100644 src/telemetry_common.c create mode 100644 src/telemetry_common.h create mode 100644 src/telemetry_frsky.h diff --git a/Makefile b/Makefile index 04055fe83..9e50b1ca8 100644 --- a/Makefile +++ b/Makefile @@ -55,6 +55,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \ sbus.c \ sumd.c \ spektrum.c \ + telemetry_common.c \ telemetry_frsky.c \ drv_gpio.c \ drv_i2c.c \ diff --git a/src/main.c b/src/main.c index 24907b2d1..86f18bdfa 100755 --- a/src/main.c +++ b/src/main.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +#include "telemetry_common.h" + core_t core; extern rcReadRawDataPtr rcReadRawFunc; diff --git a/src/mw.c b/src/mw.c index 026ad7e31..a4bcda037 100755 --- a/src/mw.c +++ b/src/mw.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +#include "telemetry_common.h" + // June 2013 V2.2-dev flags_t f; diff --git a/src/mw.h b/src/mw.h index 9fd5e2c84..e71347706 100755 --- a/src/mw.h +++ b/src/mw.h @@ -477,7 +477,3 @@ void GPS_reset_nav(void); void GPS_set_next_wp(int32_t* lat, int32_t* lon); int32_t wrap_18000(int32_t error); -// telemetry -void initTelemetry(void); -void updateTelemetryState(void); -void sendTelemetry(void); diff --git a/src/serial.c b/src/serial.c index 8a3bba559..ae359c065 100755 --- a/src/serial.c +++ b/src/serial.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +#include "telemetry_common.h" + // Multiwii Serial Protocol 0 #define MSP_VERSION 0 #define CAP_PLATFORM_32BIT ((uint32_t)1 << 31) diff --git a/src/telemetry_common.c b/src/telemetry_common.c new file mode 100644 index 000000000..79398530b --- /dev/null +++ b/src/telemetry_common.c @@ -0,0 +1,51 @@ +#include "board.h" +#include "mw.h" + +#include "telemetry_frsky.h" + +void initTelemetry(void) +{ + // Sanity check for softserial vs. telemetry port + if (!feature(FEATURE_SOFTSERIAL)) + mcfg.telemetry_softserial = TELEMETRY_UART; + + if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL_1) + core.telemport = &(softSerialPorts[0].port); + else if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL_2) + core.telemport = &(softSerialPorts[1].port); + else + core.telemport = core.mainport; +} + +void updateTelemetryState(void) { + updateFrSkyTelemetryState(); +} + +bool isTelemetryEnabled(void) +{ + bool telemetryCurrentlyEnabled = true; + + if (mcfg.telemetry_softserial == TELEMETRY_UART) { + if (!mcfg.telemetry_switch) + telemetryCurrentlyEnabled = f.ARMED; + else + telemetryCurrentlyEnabled = rcOptions[BOXTELEMETRY]; + } + + return telemetryCurrentlyEnabled; +} + +bool isFrSkyTelemetryEnabled(void) +{ + return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY; +} + +void sendTelemetry(void) +{ + if (!isTelemetryEnabled()) + return; + + if (isFrSkyTelemetryEnabled()) { + sendFrSkyTelemetry(); + } +} diff --git a/src/telemetry_common.h b/src/telemetry_common.h new file mode 100644 index 000000000..9a4129968 --- /dev/null +++ b/src/telemetry_common.h @@ -0,0 +1,17 @@ +/* + * telemetry_common.h + * + * Created on: 6 Apr 2014 + * Author: Hydra + */ + +#ifndef TELEMETRY_COMMON_H_ +#define TELEMETRY_COMMON_H_ + +// telemetry +void initTelemetry(void); +void updateTelemetryState(void); +void sendTelemetry(void); +bool isTelemetryEnabled(void); + +#endif /* TELEMETRY_COMMON_H_ */ diff --git a/src/telemetry_frsky.c b/src/telemetry_frsky.c index 54c93b258..ade5fdb13 100644 --- a/src/telemetry_frsky.c +++ b/src/telemetry_frsky.c @@ -4,6 +4,9 @@ #include "board.h" #include "mw.h" +#include "telemetry_common.h" +#include "telemetry_frsky.h" + #define CYCLETIME 125 #define PROTOCOL_HEADER 0x5E @@ -212,56 +215,28 @@ static void sendHeading(void) serialize16(0); } -static bool telemetryEnabled = false; +static bool frSkyTelemetryEnabled = false; -void initTelemetry(void) +bool shouldChangeTelemetryStateNow(bool frSkyTelemetryCurrentlyEnabled) { - // Sanity check for softserial vs. telemetry port - if (!feature(FEATURE_SOFTSERIAL)) - mcfg.telemetry_softserial = TELEMETRY_UART; - - if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL_1) - core.telemport = &(softSerialPorts[0].port); - else if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL_2) - core.telemport = &(softSerialPorts[1].port); - else - core.telemport = core.mainport; + return frSkyTelemetryCurrentlyEnabled != frSkyTelemetryEnabled; } -bool isTelemetryEnabled() +void updateFrSkyTelemetryState(void) { - bool telemetryCurrentlyEnabled = true; + bool frSkyTelemetryCurrentlyEnabled = isTelemetryEnabled(); - if (mcfg.telemetry_softserial == TELEMETRY_UART) { - if (!mcfg.telemetry_switch) - telemetryCurrentlyEnabled = f.ARMED; - else - telemetryCurrentlyEnabled = rcOptions[BOXTELEMETRY]; - } - - return telemetryCurrentlyEnabled; -} - -bool shouldChangeTelemetryStateNow(bool telemetryCurrentlyEnabled) -{ - return telemetryCurrentlyEnabled != telemetryEnabled; -} - -void updateTelemetryState(void) -{ - bool telemetryCurrentlyEnabled = isTelemetryEnabled(); - - if (!shouldChangeTelemetryStateNow(telemetryCurrentlyEnabled)) { + if (!shouldChangeTelemetryStateNow(frSkyTelemetryCurrentlyEnabled)) { return; } if (mcfg.telemetry_softserial == TELEMETRY_UART && mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY) { - if (telemetryCurrentlyEnabled) + if (frSkyTelemetryCurrentlyEnabled) serialInit(9600); else serialInit(mcfg.serial_baudrate); } - telemetryEnabled = telemetryCurrentlyEnabled; + frSkyTelemetryEnabled = frSkyTelemetryCurrentlyEnabled; } static uint32_t lastCycleTime = 0; @@ -325,18 +300,3 @@ void sendFrSkyTelemetry(void) } } -bool isFrSkyTelemetryEnabled(void) -{ - return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY; -} - -void sendTelemetry(void) -{ - if (!isTelemetryEnabled()) - return; - - if (isFrSkyTelemetryEnabled()) { - sendFrSkyTelemetry(); - } -} - diff --git a/src/telemetry_frsky.h b/src/telemetry_frsky.h new file mode 100644 index 000000000..343b09be9 --- /dev/null +++ b/src/telemetry_frsky.h @@ -0,0 +1,14 @@ +/* + * telemetry_frsky.h + * + * Created on: 6 Apr 2014 + * Author: Hydra + */ + +#ifndef TELEMETRY_FRSKY_H_ +#define TELEMETRY_FRSKY_H_ + +void sendFrSkyTelemetry(void); +void updateFrSkyTelemetryState(void); + +#endif /* TELEMETRY_FRSKY_H_ */