Telemetry Forwarding

Makefile

@@ -297,6 +297,7 @@ HIGHEND_SRC = \
 		   telemetry/nmea.c \
 		   telemetry/ltm.c \
 		   telemetry/position_estimation_log.c \
+		   telemetry/forward.c \
 		   sensors/sonar.c \
 		   sensors/barometer.c
 

src/main/io/serial.h

@@ -38,6 +38,7 @@ typedef enum {
 	FUNCTION_TELEMETRY_NMEA  	 = (1 << 10),  // 1024
 	FUNCTION_TELEMETRY_LTM		 = (1 << 11),  // 2048
 	FUNCTION_TELEMETRY_POSEST	 = (1 << 12),  // 4096
+	FUNCTION_TELEMETRY_FORWARD   = (1 << 13), // 8192
 } serialPortFunction_e;
 
 typedef enum {

src/main/telemetry/forward.c

@@ -0,0 +1,124 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+
+#ifdef TELEMETRY
+
+#include "common/maths.h"
+#include "common/axis.h"
+
+#include "drivers/system.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+#include "drivers/gpio.h"
+#include "drivers/timer.h"
+#include "drivers/serial.h"
+
+
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+#include "sensors/gyro.h"
+#include "sensors/barometer.h"
+#include "sensors/battery.h"
+
+#include "io/serial.h"
+#include "io/rc_controls.h"
+#include "io/gps.h"
+
+#include "rx/rx.h"
+
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/altitudehold.h"
+
+#include "config/runtime_config.h"
+#include "config/config.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/forward.h"
+
+static serialPort_t *forwardPort = NULL;
+static serialPortConfig_t *portConfig;
+
+#define FORWARD_BAUDRATE 57600
+#define FORWARD_INITIAL_PORT_MODE MODE_RXTX
+
+static telemetryConfig_t *telemetryConfig;
+static bool forwardTelemetryEnabled =  false;
+static portSharing_e forwardPortSharing;
+
+#define CYCLETIME             125
+
+static uint32_t lastCycleTime = 0;
+static uint8_t cycleNum = 0;
+
+void initForwardTelemetry(telemetryConfig_t *initialTelemetryConfig)
+{
+    telemetryConfig = initialTelemetryConfig;
+    portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FORWARD);
+    forwardPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FORWARD);
+}
+
+void freeForwardTelemetryPort(void)
+{
+    closeSerialPort(forwardPort);
+    forwardPort = NULL;
+    forwardTelemetryEnabled = false;
+}
+
+void configureForwardTelemetryPort(void)
+{
+    if (!portConfig) {
+        return;
+    }
+
+    forwardPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FORWARD, NULL, FORWARD_BAUDRATE, FORWARD_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
+    if (!forwardPort) {
+        return;
+    }
+
+    forwardTelemetryEnabled = true;
+}
+
+void checkForwardTelemetryState(void)
+{
+    bool newTelemetryEnabledValue = telemetryDetermineEnabledState(forwardPortSharing);
+
+    if (newTelemetryEnabledValue == forwardTelemetryEnabled) {
+        return;
+    }
+
+    if (newTelemetryEnabledValue)
+        configureForwardTelemetryPort();
+    else
+        freeForwardTelemetryPort();
+}
+
+void handleForwardTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
+{
+    if (!forwardTelemetryEnabled) {
+        return;
+    }
+
+    while (serialRxBytesWaiting(forwardPort) > 0) {
+            uint8_t c = serialRead(forwardPort);
+            evaluateOtherData(forwardPort,c);
+    }
+
+
+}
+
+void forwardTelemetry(uint8_t c){
+    serialWrite(forwardPort, c);
+}
+
+bool forwardEnabled(void){
+    return(forwardTelemetryEnabled);
+}
+
+
+#endif
+

src/main/telemetry/forward.h

@@ -0,0 +1,14 @@
+#include "rx/rx.h"
+
+#ifndef TELEMETRY_FORWARD_
+#define TELEMETRY_FORWARD_
+
+void handleForwardTelemetryvoid(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
+void checkForwardTelemetryState(void);
+
+void initForwardTelemetry(telemetryConfig_t *telemetryConfig);
+void configureForwardTelemetryPort(void);
+void freeForwardTelemetryPort(void);
+bool forwardEnabled(void);
+
+#endif /* TELEMETRY_FORWARD_ */

src/main/telemetry/telemetry.c

@@ -45,6 +45,7 @@
 #include "telemetry/nmea.h"
 #include "telemetry/ltm.h"
 #include "telemetry/position_estimation_log.h"
+#include "telemetry/forward.h"
 
 static telemetryConfig_t *telemetryConfig;
 
@@ -64,7 +65,7 @@ void telemetryInit(void)
     initNMEATelemetry(telemetryConfig);
     initLtmTelemetry(telemetryConfig);
     initPOSESTTelemetry(telemetryConfig);
-
+    initForwardTelemetry(telemetryConfig);
     telemetryCheckState();
 }
 
@@ -93,6 +94,7 @@ void telemetryCheckState(void)
     checkNMEATelemetryState();
     checkLtmTelemetryState();
     checkPOSESTTelemetryState();
+    checkForwardTelemetryState();
 }
 
 void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
@@ -106,6 +108,7 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
     handleNMEATelemetry();
     handleLtmTelemetry();
     handlePOSESTTelemetry();
+    handleForwardTelemetry();
 }
 
 #endif

src/main/tracker/telemetry.c

@@ -106,6 +106,10 @@ void encodeTargetData(uint8_t c) {
 	    crossfire_encodeTargetData(c);
 	else if(PROTOCOL(TP_PITLAB))
 		pitlab_encodeTargetData(c);
+
+	if(forwardEnabled()){
+	    forwardTelemetry(c);
+	}
 }
 
 void gps_encodeTargetData(uint8_t c) {

src/main/version.h

@@ -17,7 +17,7 @@
 
 #define FC_VERSION_MAJOR            11 // increment when a major release is made (big new feature, etc)
 #define FC_VERSION_MINOR            1 // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      0 // increment when a bug is fixed
+#define FC_VERSION_PATCH_LEVEL      1 // increment when a bug is fixed
 
 #define STR_HELPER(x) #x
 #define STR(x) STR_HELPER(x)