From 2a4c471c9d87a6912575b134228db4ad2a2a7fc8 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Ra=C3=BAl=20Ortega?= Date: Mon, 12 Apr 2021 00:33:19 +0200 Subject: [PATCH] Telemetry Forwarding --- Makefile | 1 + src/main/io/serial.h | 1 + src/main/telemetry/forward.c | 124 +++++++++++++++++++++++++++++++++++++++++ src/main/telemetry/forward.h | 14 +++++ src/main/telemetry/telemetry.c | 5 +- src/main/tracker/telemetry.c | 4 ++ src/main/version.h | 2 +- 7 files changed, 149 insertions(+), 2 deletions(-) create mode 100644 src/main/telemetry/forward.c create mode 100644 src/main/telemetry/forward.h diff --git a/Makefile b/Makefile index 35a1361..9506324 100755 --- a/Makefile +++ b/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 diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 28e546d..a5c50ac 100755 --- a/src/main/io/serial.h +++ b/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 { diff --git a/src/main/telemetry/forward.c b/src/main/telemetry/forward.c new file mode 100644 index 0000000..7142c1a --- /dev/null +++ b/src/main/telemetry/forward.c @@ -0,0 +1,124 @@ +#include +#include +#include + +#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 + diff --git a/src/main/telemetry/forward.h b/src/main/telemetry/forward.h new file mode 100644 index 0000000..26e7c5d --- /dev/null +++ b/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_ */ diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 30d234a..c36a53b 100755 --- a/src/main/telemetry/telemetry.c +++ b/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 diff --git a/src/main/tracker/telemetry.c b/src/main/tracker/telemetry.c index aefec68..a48874a 100755 --- a/src/main/tracker/telemetry.c +++ b/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) { diff --git a/src/main/version.h b/src/main/version.h index 71485b7..77b056e 100755 --- a/src/main/version.h +++ b/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) -- 2.11.4.GIT