Move common telemetry code to telemetry_common.c/.h

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 \

src/main.c

@@ -1,6 +1,8 @@
 #include "board.h"
 #include "mw.h"
 
+#include "telemetry_common.h"
+
 core_t core;
 
 extern rcReadRawDataPtr rcReadRawFunc;

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;

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);

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)

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();
+    }
+}

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_ */

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
+    return frSkyTelemetryCurrentlyEnabled != frSkyTelemetryEnabled;
-    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;
 }
 
-bool isTelemetryEnabled()
+void updateFrSkyTelemetryState(void)
 {
-    bool telemetryCurrentlyEnabled = true;
+    bool frSkyTelemetryCurrentlyEnabled = isTelemetryEnabled();
 
-    if (mcfg.telemetry_softserial == TELEMETRY_UART) {
+    if (!shouldChangeTelemetryStateNow(frSkyTelemetryCurrentlyEnabled)) {
-        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)) {
         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();
-    }
-}
-

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_ */