diff --git a/Makefile b/Makefile
index ce2fd41072..2cee3893df 100644
--- a/Makefile
+++ b/Makefile
@@ -55,7 +55,9 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
sbus.c \
sumd.c \
spektrum.c \
- telemetry.c \
+ telemetry_common.c \
+ telemetry_frsky.c \
+ telemetry_hott.c \
drv_gpio.c \
drv_i2c.c \
drv_i2c_soft.c \
diff --git a/baseflight.uvproj b/baseflight.uvproj
index 1fd8dc25c1..2eea4a6ddf 100755
--- a/baseflight.uvproj
+++ b/baseflight.uvproj
@@ -1345,9 +1345,9 @@
.\src\buzzer.c
- telemetry.c
+ telemetry_frsky.c
1
- .\src\telemetry.c
+ .\src\telemetry_frsky.c
utils.c
diff --git a/src/board.h b/src/board.h
index f514838c4f..099e6620f9 100755
--- a/src/board.h
+++ b/src/board.h
@@ -90,12 +90,30 @@ typedef enum {
GPS_MTK_NMEA,
GPS_MTK_BINARY,
GPS_MAG_BINARY,
+ GPS_HARDWARE_MAX = GPS_MAG_BINARY,
} GPSHardware;
typedef enum {
- TELEMETRY_UART = 0,
- TELEMETRY_SOFTSERIAL,
-} TelemetrySerial;
+ GPS_BAUD_115200 = 0,
+ GPS_BAUD_57600,
+ GPS_BAUD_38400,
+ GPS_BAUD_19200,
+ GPS_BAUD_9600,
+ GPS_BAUD_MAX = GPS_BAUD_9600
+} GPSBaudRates;
+
+typedef enum {
+ TELEMETRY_PROVIDER_FRSKY = 0,
+ TELEMETRY_PROVIDER_HOTT,
+ TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_HOTT
+} TelemetryProvider;
+
+typedef enum {
+ TELEMETRY_PORT_UART = 0,
+ TELEMETRY_PORT_SOFTSERIAL_1, // Requires FEATURE_SOFTSERIAL
+ TELEMETRY_PORT_SOFTSERIAL_2, // Requires FEATURE_SOFTSERIAL
+ TELEMETRY_PORT_MAX = TELEMETRY_PORT_SOFTSERIAL_2
+} TelemetryPort;
typedef enum {
X = 0,
diff --git a/src/cli.c b/src/cli.c
index aab69f04c7..54a50451e0 100644
--- a/src/cli.c
+++ b/src/cli.c
@@ -30,6 +30,9 @@ extern const char rcChannelLetters[];
// from mixer.c
extern int16_t motor_disarmed[MAX_MOTORS];
+// signal that we're in cli mode
+uint8_t cliMode = 0;
+
// buffer
static char cliBuffer[48];
static uint32_t bufferIndex = 0;
@@ -51,7 +54,7 @@ static const char * const mixerNames[] = {
static const char * const featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP",
"SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS",
- "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D",
+ "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D",
NULL
};
@@ -130,10 +133,11 @@ const clivalue_t valueTable[] = {
{ "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 1200, 19200 },
{ "softserial_1_inverted", VAR_UINT8, &mcfg.softserial_1_inverted, 0, 1 },
{ "softserial_2_inverted", VAR_UINT8, &mcfg.softserial_2_inverted, 0, 1 },
- { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
- { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, 0, 4 },
+ { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, GPS_HARDWARE_MAX },
+ { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, 0, GPS_BAUD_MAX },
{ "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 3 },
- { "telemetry_softserial", VAR_UINT8, &mcfg.telemetry_softserial, 0, 1 },
+ { "telemetry_provider", VAR_UINT8, &mcfg.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
+ { "telemetry_port", VAR_UINT8, &mcfg.telemetry_port, 0, TELEMETRY_PORT_MAX },
{ "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 },
{ "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
diff --git a/src/cli.h b/src/cli.h
new file mode 100644
index 0000000000..f21bceb439
--- /dev/null
+++ b/src/cli.h
@@ -0,0 +1,13 @@
+/*
+ * cli.h
+ *
+ * Created on: 6 Apr 2014
+ * Author: Hydra
+ */
+
+#ifndef CLI_H_
+#define CLI_H_
+
+extern uint8_t cliMode;
+
+#endif /* CLI_H_ */
diff --git a/src/config.c b/src/config.c
index fc0af99166..2830ddc879 100755
--- a/src/config.c
+++ b/src/config.c
@@ -192,7 +192,8 @@ static void resetConf(void)
mcfg.vbatmincellvoltage = 33;
mcfg.power_adc_channel = 0;
mcfg.serialrx_type = 0;
- mcfg.telemetry_softserial = 0;
+ mcfg.telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
+ mcfg.telemetry_port = TELEMETRY_PORT_UART;
mcfg.telemetry_switch = 0;
mcfg.midrc = 1500;
mcfg.mincheck = 1100;
@@ -212,10 +213,10 @@ static void resetConf(void)
mcfg.servo_pwm_rate = 50;
// gps/nav stuff
mcfg.gps_type = GPS_NMEA;
- mcfg.gps_baudrate = 0;
+ mcfg.gps_baudrate = GPS_BAUD_115200;
// serial (USART1) baudrate
mcfg.serial_baudrate = 115200;
- mcfg.softserial_baudrate = 19200;
+ mcfg.softserial_baudrate = 9600;
mcfg.softserial_1_inverted = 0;
mcfg.softserial_2_inverted = 0;
mcfg.looptime = 3500;
diff --git a/src/drv_serial.c b/src/drv_serial.c
index ee55de4b35..93f9c23996 100644
--- a/src/drv_serial.c
+++ b/src/drv_serial.c
@@ -38,3 +38,8 @@ inline bool isSerialTransmitBufferEmpty(serialPort_t *instance)
return instance->vTable->isSerialTransmitBufferEmpty(instance);
}
+inline void serialSetMode(serialPort_t *instance, portMode_t mode)
+{
+ instance->vTable->setMode(instance, mode);
+}
+
diff --git a/src/drv_serial.h b/src/drv_serial.h
index 6d83b56c79..22adee4ce2 100644
--- a/src/drv_serial.h
+++ b/src/drv_serial.h
@@ -38,12 +38,15 @@ struct serialPortVTable {
void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
+
+ void (*setMode)(serialPort_t *instance, portMode_t mode);
};
inline void serialWrite(serialPort_t *instance, uint8_t ch);
inline uint8_t serialTotalBytesWaiting(serialPort_t *instance);
inline uint8_t serialRead(serialPort_t *instance);
inline void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
+void serialSetMode(serialPort_t *instance, portMode_t mode);
inline bool isSerialTransmitBufferEmpty(serialPort_t *instance);
void serialPrint(serialPort_t *instance, const char *str);
uint32_t serialGetBaudRate(serialPort_t *instance);
diff --git a/src/drv_softserial.c b/src/drv_softserial.c
index 7ebc4e9da9..44eb7882ed 100644
--- a/src/drv_softserial.c
+++ b/src/drv_softserial.c
@@ -120,20 +120,25 @@ void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP);
}
-void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, uint8_t inverted)
+void resetBuffers(softSerial_t *softSerial)
{
- softSerial->port.vTable = softSerialVTable;
- softSerial->port.mode = MODE_RXTX;
-
softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
softSerial->port.rxBuffer = softSerial->rxBuffer;
softSerial->port.rxBufferTail = 0;
softSerial->port.rxBufferHead = 0;
softSerial->port.txBuffer = softSerial->txBuffer;
- softSerial->port.txBufferSize = SOFT_SERIAL_BUFFER_SIZE,
+ softSerial->port.txBufferSize = SOFT_SERIAL_BUFFER_SIZE;
softSerial->port.txBufferTail = 0;
softSerial->port.txBufferHead = 0;
+}
+
+void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, uint8_t inverted)
+{
+ softSerial->port.vTable = softSerialVTable;
+ softSerial->port.mode = MODE_RXTX;
+
+ resetBuffers(softSerial);
softSerial->isTransmittingData = false;
@@ -237,6 +242,8 @@ void processTxState(softSerial_t *softSerial)
softSerial->isTransmittingData = false;
}
+
+
enum {
TRAILING,
LEADING
@@ -268,6 +275,10 @@ void prepareForNextRxByte(softSerial_t *softSerial)
void extractAndStoreRxByte(softSerial_t *softSerial)
{
+ if ((softSerial->port.mode & MODE_RX) == 0) {
+ return;
+ }
+
uint8_t haveStartBit = (softSerial->internalRxBuffer & START_BIT_MASK) == 0;
uint8_t haveStopBit = (softSerial->internalRxBuffer & STOP_BIT_MASK) == 1;
@@ -317,6 +328,10 @@ void onSerialRxPinChange(uint8_t portIndex, uint16_t capture)
{
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
+ if ((softSerial->port.mode & MODE_RX) == 0) {
+ return;
+ }
+
if (softSerial->isSearchingForStartBit) {
// synchronise bit counter
// FIXME this reduces functionality somewhat as receiving breaks concurrent transmission on all ports because
@@ -353,6 +368,10 @@ void onSerialRxPinChange(uint8_t portIndex, uint16_t capture)
uint8_t softSerialTotalBytesWaiting(serialPort_t *instance)
{
+ if ((instance->mode & MODE_RX) == 0) {
+ return 0;
+ }
+
int availableBytes;
softSerial_t *softSerial = (softSerial_t *)instance;
if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) {
@@ -379,6 +398,11 @@ static void moveHeadToNextByte(softSerial_t *softSerial)
uint8_t softSerialReadByte(serialPort_t *instance)
{
char b;
+
+ if ((instance->mode & MODE_RX) == 0) {
+ return 0;
+ }
+
if (softSerialTotalBytesWaiting(instance) == 0) {
return 0;
}
@@ -391,6 +415,10 @@ uint8_t softSerialReadByte(serialPort_t *instance)
void softSerialWriteByte(serialPort_t *s, uint8_t ch)
{
+ if ((s->mode & MODE_TX) == 0) {
+ return;
+ }
+
s->txBuffer[s->txBufferHead] = ch;
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
}
@@ -400,6 +428,11 @@ void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
// not implemented.
}
+void softSerialSetMode(serialPort_t *instance, portMode_t mode)
+{
+ instance->mode = mode;
+}
+
bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance)
{
return instance->txBufferHead == instance->txBufferTail;
@@ -407,10 +440,11 @@ bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance)
const struct serialPortVTable softSerialVTable[] = {
{
- softSerialWriteByte,
+ softSerialWriteByte,
softSerialTotalBytesWaiting,
softSerialReadByte,
softSerialSetBaudRate,
- isSoftSerialTransmitBufferEmpty
+ isSoftSerialTransmitBufferEmpty,
+ softSerialSetMode,
}
};
diff --git a/src/drv_uart.c b/src/drv_uart.c
index 7a1727f431..0bdc6253b2 100755
--- a/src/drv_uart.c
+++ b/src/drv_uart.c
@@ -202,6 +202,12 @@ void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
s->port.baudRate = baudRate;
}
+void uartSetMode(serialPort_t *s, portMode_t mode)
+{
+ // not implemented.
+}
+
+
static void uartStartTxDMA(uartPort_t *s)
{
s->txDMAChannel->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail];
@@ -273,7 +279,8 @@ const struct serialPortVTable uartVTable[] = {
uartTotalBytesWaiting,
uartRead,
uartSetBaudRate,
- isUartTransmitBufferEmpty
+ isUartTransmitBufferEmpty,
+ uartSetMode,
}
};
diff --git a/src/gps.c b/src/gps.c
index 8a0b6d1a7b..98b34d89e7 100644
--- a/src/gps.c
+++ b/src/gps.c
@@ -8,22 +8,24 @@
// GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
#define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below
-#define GPS_INIT_ENTRIES (5)
+#define GPS_INIT_ENTRIES (GPS_BAUD_MAX + 1)
#define GPS_BAUD_DELAY (100)
typedef struct gpsInitData_t {
+ uint8_t index;
uint32_t baudrate;
const char *ubx;
const char *mtk;
} gpsInitData_t;
+// NMEA will cycle through these until valid data is received
static const gpsInitData_t gpsInitData[] = {
- { 115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
- { 57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
- { 38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
- { 19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
+ { GPS_BAUD_115200, 115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
+ { GPS_BAUD_57600, 57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
+ { GPS_BAUD_38400, 38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
+ { GPS_BAUD_19200, 19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
// 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
- { 9600, "", "" }
+ { GPS_BAUD_9600, 9600, "", "" }
};
static const uint8_t ubloxInit[] = {
@@ -41,20 +43,6 @@ static const uint8_t ubloxInit[] = {
0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz
};
-static const char *mtkNMEAInit[] = {
- "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n", // only GGA and RMC sentence
- "$PMTK220,200*2C\r\n" // 5 Hz update rate
-};
-
-static const char *mtkBinaryInit[] = {
- "$PMTK319,1*24\r\n", // SBAS Integrity Mode
- "$PMTK220,200*2C\r\n", // 5 Hz update rate
- "$PMTK397,0*23\r\n", // NAVTHRES_OFF
- "$PMTK313,1*2E\r\n", // SBAS_ON
- "$PMTK301,2*2E\r\n", // WAAS_ON
- "$PGCMD,16,0,0,0,0,0*6A\r\n" // Binary ON
-};
-
enum {
GPS_UNKNOWN,
GPS_INITIALIZING,
@@ -88,7 +76,7 @@ static void gpsSetState(uint8_t state)
gpsData.state_ts = millis();
}
-void gpsInit(uint8_t baudrate)
+void gpsInit(uint8_t baudrateIndex)
{
portMode_t mode = MODE_RXTX;
@@ -97,7 +85,7 @@ void gpsInit(uint8_t baudrate)
if (!feature(FEATURE_GPS))
return;
- gpsData.baudrateIndex = baudrate;
+ gpsData.baudrateIndex = baudrateIndex;
gpsData.lastMessage = millis();
gpsData.errors = 0;
// only RX is needed for NMEA-style GPS
@@ -105,7 +93,7 @@ void gpsInit(uint8_t baudrate)
mode = MODE_RX;
gpsSetPIDs();
- core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrate].baudrate, mode);
+ core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrateIndex].baudrate, mode);
// signal GPS "thread" to initialize when it gets to it
gpsSetState(GPS_INITIALIZING);
}
diff --git a/src/main.c b/src/main.c
index 818dba6a2b..bd0877ecab 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/mixer.c b/src/mixer.c
index 364a1e2ae0..d982c8e4dd 100755
--- a/src/mixer.c
+++ b/src/mixer.c
@@ -304,8 +304,6 @@ void writeServos(void)
}
}
-extern uint8_t cliMode;
-
void writeMotors(void)
{
uint8_t i;
diff --git a/src/mw.c b/src/mw.c
index 4869bf036a..b75cca42ed 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -1,6 +1,9 @@
#include "board.h"
#include "mw.h"
+#include "cli.h"
+#include "telemetry_common.h"
+
// June 2013 V2.2-dev
flags_t f;
@@ -175,9 +178,8 @@ void annexCode(void)
LED0_OFF;
if (f.ARMED)
LED0_ON;
- // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. With telemetry_softserial>0 telemetry is always enabled, also see updateTelemetryState()
- if (feature(FEATURE_TELEMETRY))
- updateTelemetryState();
+
+ checkTelemetryState();
}
#ifdef LEDRING
@@ -202,6 +204,10 @@ void annexCode(void)
serialCom();
+ if (!cliMode && feature(FEATURE_TELEMETRY)) {
+ handleTelemetry();
+ }
+
if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime;
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
diff --git a/src/mw.h b/src/mw.h
index 5fcb96de90..a3c5afd741 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -272,8 +272,8 @@ typedef struct master_t {
uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable.
// gps-related stuff
- uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2: MTK NMEA 3: MTK Binary
- int8_t gps_baudrate; // GPS baudrate, 0: 115200, 1: 57600, 2: 38400, 3: 19200, 4: 9600. NMEA will cycle through these until valid data is received
+ uint8_t gps_type; // See GPSHardware enum.
+ int8_t gps_baudrate; // See GPSBaudRates enum.
uint32_t serial_baudrate;
@@ -281,7 +281,8 @@ typedef struct master_t {
uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1
uint8_t softserial_2_inverted; // use inverted softserial input and output signals on port 2
- uint8_t telemetry_softserial; // Serial to use for Telemetry. 0:USART1, 1:SoftSerial1 (Enable FEATURE_SOFTSERIAL first)
+ uint8_t telemetry_provider; // See TelemetryProvider enum.
+ uint8_t telemetry_port; // See TelemetryPort enum.
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
config_t profile[3]; // 3 separate profiles
uint8_t current_profile; // currently loaded profile
@@ -477,7 +478,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 8a3bba559d..719ffe93af 100755
--- a/src/serial.c
+++ b/src/serial.c
@@ -1,6 +1,9 @@
#include "board.h"
#include "mw.h"
+#include "cli.h"
+#include "telemetry_common.h"
+
// Multiwii Serial Protocol 0
#define MSP_VERSION 0
#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
@@ -112,8 +115,6 @@ static const char pidnames[] =
static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
static uint8_t cmdMSP;
-// signal that we're in cli mode
-uint8_t cliMode = 0;
void serialize32(uint32_t a)
{
@@ -694,7 +695,4 @@ void serialCom(void)
c_state = IDLE;
}
}
- if (!cliMode && feature(FEATURE_TELEMETRY)) { // The first condition should never evaluate to true but I'm putting it here anyway - silpstream
- sendTelemetry();
- }
}
diff --git a/src/telemetry_common.c b/src/telemetry_common.c
new file mode 100644
index 0000000000..6302a1855f
--- /dev/null
+++ b/src/telemetry_common.c
@@ -0,0 +1,136 @@
+#include "board.h"
+#include "mw.h"
+
+#include "telemetry_frsky.h"
+#include "telemetry_hott.h"
+
+static bool isTelemetryConfigurationValid = false; // flag used to avoid repeated configuration checks
+
+bool isTelemetryProviderFrSky(void)
+{
+ return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
+}
+
+bool isTelemetryProviderHoTT(void)
+{
+ return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT;
+}
+
+bool canUseTelemetryWithCurrentConfiguration(void)
+{
+
+ if (!feature(FEATURE_TELEMETRY)) {
+ return false;
+ }
+
+ if (!feature(FEATURE_SOFTSERIAL)) {
+ if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) {
+ // softserial feature must be enabled to use telemetry on softserial ports
+ return false;
+ }
+ }
+
+ if (isTelemetryProviderHoTT()) {
+ if (mcfg.telemetry_port == TELEMETRY_PORT_UART) {
+ // HoTT requires a serial port that supports RX/TX mode swapping
+ return false;
+ }
+ }
+
+ return true;
+}
+
+void initTelemetry(void)
+{
+ // Force telemetry to uart when softserial disabled
+ if (!feature(FEATURE_SOFTSERIAL))
+ mcfg.telemetry_port = TELEMETRY_PORT_UART;
+
+ isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration();
+
+ if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1)
+ core.telemport = &(softSerialPorts[0].port);
+ else if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2)
+ core.telemport = &(softSerialPorts[1].port);
+ else
+ core.telemport = core.mainport;
+
+ checkTelemetryState();
+}
+
+static bool telemetryEnabled = false;
+
+bool determineNewTelemetryEnabledState(void)
+{
+ bool enabled = true;
+
+ if (mcfg.telemetry_port == TELEMETRY_PORT_UART) {
+ if (!mcfg.telemetry_switch)
+ enabled = f.ARMED;
+ else
+ enabled = rcOptions[BOXTELEMETRY];
+ }
+
+ return enabled;
+}
+
+bool shouldChangeTelemetryStateNow(bool newState)
+{
+ return newState != telemetryEnabled;
+}
+
+static void configureTelemetryPort(void)
+{
+ if (isTelemetryProviderFrSky()) {
+ configureFrSkyTelemetryPort();
+ }
+
+ if (isTelemetryProviderHoTT()) {
+ configureHoTTTelemetryPort();
+ }
+}
+
+void freeTelemetryPort(void)
+{
+ if (isTelemetryProviderFrSky()) {
+ freeFrSkyTelemetryPort();
+ }
+
+ if (isTelemetryProviderHoTT()) {
+ freeHoTTTelemetryPort();
+ }
+}
+
+void checkTelemetryState(void)
+{
+ if (!isTelemetryConfigurationValid) {
+ return;
+ }
+
+ bool newEnabledState = determineNewTelemetryEnabledState();
+
+ if (!shouldChangeTelemetryStateNow(newEnabledState)) {
+ return;
+ }
+
+ if (newEnabledState)
+ configureTelemetryPort();
+ else
+ freeTelemetryPort();
+
+ telemetryEnabled = newEnabledState;
+}
+
+void handleTelemetry(void)
+{
+ if (!isTelemetryConfigurationValid || !determineNewTelemetryEnabledState())
+ return;
+
+ if (isTelemetryProviderFrSky()) {
+ handleFrSkyTelemetry();
+ }
+
+ if (isTelemetryProviderHoTT()) {
+ handleHoTTTelemetry();
+ }
+}
diff --git a/src/telemetry_common.h b/src/telemetry_common.h
new file mode 100644
index 0000000000..0241d6415e
--- /dev/null
+++ b/src/telemetry_common.h
@@ -0,0 +1,16 @@
+/*
+ * telemetry_common.h
+ *
+ * Created on: 6 Apr 2014
+ * Author: Hydra
+ */
+
+#ifndef TELEMETRY_COMMON_H_
+#define TELEMETRY_COMMON_H_
+
+// telemetry
+void initTelemetry(void);
+void checkTelemetryState(void);
+void handleTelemetry(void);
+
+#endif /* TELEMETRY_COMMON_H_ */
diff --git a/src/telemetry.c b/src/telemetry_frsky.c
similarity index 70%
rename from src/telemetry.c
rename to src/telemetry_frsky.c
index 2be8629072..8d737d9f42 100644
--- a/src/telemetry.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,83 +215,78 @@ static void sendHeading(void)
serialize16(0);
}
-static bool telemetryEnabled = false;
-
-void initTelemetry(void)
+void freeFrSkyTelemetryPort(void)
{
- // Sanity check for softserial vs. telemetry port
- if (!feature(FEATURE_SOFTSERIAL))
- mcfg.telemetry_softserial = TELEMETRY_UART;
-
- if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL)
- core.telemport = &(softSerialPorts[0].port);
- else
- core.telemport = core.mainport;
+ if (mcfg.telemetry_port == TELEMETRY_PORT_UART) {
+ serialInit(mcfg.serial_baudrate);
+ }
}
-void updateTelemetryState(void)
+void configureFrSkyTelemetryPort(void)
{
- bool State;
- if (!mcfg.telemetry_switch)
- State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : f.ARMED;
- else
- State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : rcOptions[BOXTELEMETRY];
-
- if (State != telemetryEnabled) {
- if (mcfg.telemetry_softserial == TELEMETRY_UART) {
- if (State)
- serialInit(9600);
- else
- serialInit(mcfg.serial_baudrate);
- }
- telemetryEnabled = State;
+ if (mcfg.telemetry_port == TELEMETRY_PORT_UART) {
+ serialInit(9600);
}
}
static uint32_t lastCycleTime = 0;
static uint8_t cycleNum = 0;
-void sendTelemetry(void)
+bool canSendFrSkyTelemetry(void)
{
- if (mcfg.telemetry_softserial == TELEMETRY_UART && ((!f.ARMED && !mcfg.telemetry_switch) || (mcfg.telemetry_switch && !rcOptions[BOXTELEMETRY])))
+ return serialTotalBytesWaiting(core.telemport) == 0;
+}
+
+bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)
+{
+ return currentMillis - lastCycleTime >= CYCLETIME;
+}
+
+void handleFrSkyTelemetry(void)
+{
+ if (!canSendFrSkyTelemetry()) {
return;
+ }
- if (serialTotalBytesWaiting(core.telemport) != 0)
+ uint32_t now = millis();
+
+ if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
return;
+ }
- if (millis() - lastCycleTime >= CYCLETIME) {
- lastCycleTime = millis();
- cycleNum++;
+ lastCycleTime = now;
- // Sent every 125ms
- sendAccel();
- sendVario();
+ cycleNum++;
+
+ // Sent every 125ms
+ sendAccel();
+ sendVario();
+ sendTelemetryTail();
+
+ if ((cycleNum % 4) == 0) { // Sent every 500ms
+ sendBaro();
+ sendHeading();
sendTelemetryTail();
+ }
- if ((cycleNum % 4) == 0) { // Sent every 500ms
- sendBaro();
- sendHeading();
- sendTelemetryTail();
+ if ((cycleNum % 8) == 0) { // Sent every 1s
+ sendTemperature1();
+
+ if (feature(FEATURE_VBAT)) {
+ sendVoltage();
+ sendVoltageAmp();
}
- if ((cycleNum % 8) == 0) { // Sent every 1s
- sendTemperature1();
+ if (sensors(SENSOR_GPS))
+ sendGPS();
- if (feature(FEATURE_VBAT)) {
- sendVoltage();
- sendVoltageAmp();
- }
+ sendTelemetryTail();
+ }
- if (sensors(SENSOR_GPS))
- sendGPS();
-
- sendTelemetryTail();
- }
-
- if (cycleNum == 40) { //Frame 3: Sent every 5s
- cycleNum = 0;
- sendTime();
- sendTelemetryTail();
- }
+ if (cycleNum == 40) { //Frame 3: Sent every 5s
+ cycleNum = 0;
+ sendTime();
+ sendTelemetryTail();
}
}
+
diff --git a/src/telemetry_frsky.h b/src/telemetry_frsky.h
new file mode 100644
index 0000000000..086632253a
--- /dev/null
+++ b/src/telemetry_frsky.h
@@ -0,0 +1,17 @@
+/*
+ * telemetry_frsky.h
+ *
+ * Created on: 6 Apr 2014
+ * Author: Hydra
+ */
+
+#ifndef TELEMETRY_FRSKY_H_
+#define TELEMETRY_FRSKY_H_
+
+void handleFrSkyTelemetry(void);
+void checkFrSkyTelemetryState(void);
+
+void configureFrSkyTelemetryPort(void);
+void freeFrSkyTelemetryPort(void);
+
+#endif /* TELEMETRY_FRSKY_H_ */
diff --git a/src/telemetry_hott.c b/src/telemetry_hott.c
new file mode 100644
index 0000000000..1569c6ddd3
--- /dev/null
+++ b/src/telemetry_hott.c
@@ -0,0 +1,274 @@
+/*
+ * telemetry_hott.c
+ *
+ * Created on: 6 Apr 2014
+ * Authors:
+ * Dominic Clifton - Hydra - Software Serial, Electronics, Hardware Integration and debugging, HoTT Code cleanup and fixes, general telemetry improvements.
+ * Carsten Giesen - cGiesen - Baseflight port
+ * Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering
+ *
+ * It should be noted that the initial cut of code that deals with the handling of requests and formatting and
+ * sending of responses comes from the MultiWii-Meets-HoTT and MultiHoTT-module projects
+ *
+ * https://github.com/obayer/MultiWii-HoTT
+ * https://github.com/oBayer/MultiHoTT-Module
+ *
+ * HoTT is implemented in Graupner equipment using a bi-directional protocol over a single wire.
+ *
+ * Generally the receiver sends a single request byte out using normal uart signals, then waits a short period for a
+ * multiple byte response and checksum byte before it sends out the next request byte.
+ * Each response byte must be send with a protocol specific delay between them.
+ *
+ * Serial ports use two wires but HoTT uses a single wire so some electronics are required so that
+ * the signals don't get mixed up. When baseflight transmits it should not receive it's own transmission.
+ *
+ * Connect as follows:
+ * HoTT TX/RX -> Serial RX (connect directly)
+ * Serial TX -> 1N4148 Diode -(| )-> HoTT TX/RX (connect via diode)
+ *
+ * The diode should be arranged to allow the data signals to flow the right way
+ * -(| )- == Diode, | indicates cathode marker.
+ *
+ * As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which digitals pins are 5V tolerant.
+ *
+ * Note: The softserial ports are not listed as 5V tolerant in the STM32F103xx data sheet pinouts and pin description
+ * section. Verify if you require a 5v/3.3v level shifters. The softserial port should not be inverted.
+ *
+ * Technically it is possible to use less components and disable softserial RX when transmitting but that is
+ * not currently supported.
+ *
+ * There is a technical discussion (in German) about HoTT here
+ * http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
+ */
+
+#include "board.h"
+#include "mw.h"
+
+#include "telemetry_hott.h"
+
+
+const uint8_t kHoTTv4BinaryPacketSize = 45;
+const uint8_t kHoTTv4TextPacketSize = 173;
+
+static void hottV4SerialWrite(uint8_t c);
+
+static void hottV4Respond(uint8_t *data, uint8_t size);
+static void hottV4FormatAndSendGPSResponse(void);
+static void hottV4GPSUpdate(void);
+static void hottV4FormatAndSendEAMResponse(void);
+static void hottV4EAMUpdateBattery(void);
+static void hottV4EAMUpdateTemperatures(void);
+bool batteryWarning;
+
+/*
+ * Sends HoTTv4 capable GPS telemetry frame.
+ */
+
+void hottV4FormatAndSendGPSResponse(void)
+{
+ memset(&HoTTV4GPSModule, 0, sizeof(HoTTV4GPSModule));
+
+ // Minimum data set for EAM
+ HoTTV4GPSModule.startByte = 0x7C;
+ HoTTV4GPSModule.sensorID = HOTTV4_GPS_SENSOR_ID;
+ HoTTV4GPSModule.sensorTextID = HOTTV4_GPS_SENSOR_TEXT_ID;
+ HoTTV4GPSModule.endByte = 0x7D;
+
+ // Reset alarms
+ HoTTV4GPSModule.alarmTone = 0x0;
+ HoTTV4GPSModule.alarmInverse1 = 0x0;
+
+ hottV4GPSUpdate();
+
+ hottV4Respond((uint8_t*)&HoTTV4GPSModule, sizeof(HoTTV4GPSModule));
+}
+
+void hottV4GPSUpdate(void)
+{
+ // Number of Satelites
+ HoTTV4GPSModule.GPSNumSat=GPS_numSat;
+ if (f.GPS_FIX > 0) {
+ // GPS fix
+ HoTTV4GPSModule.GPS_fix = 0x66; // Displays a 'f' for fix
+
+ // latitude
+ HoTTV4GPSModule.LatitudeNS=(GPS_coord[LAT]<0);
+ uint8_t deg = GPS_coord[LAT] / 100000;
+ uint32_t sec = (GPS_coord[LAT] - (deg * 100000)) * 6;
+ uint8_t min = sec / 10000;
+ sec = sec % 10000;
+ uint16_t degMin = (deg * 100) + min;
+ HoTTV4GPSModule.LatitudeMinLow = degMin;
+ HoTTV4GPSModule.LatitudeMinHigh = degMin >> 8;
+ HoTTV4GPSModule.LatitudeSecLow = sec;
+ HoTTV4GPSModule.LatitudeSecHigh = sec >> 8;
+
+ // longitude
+ HoTTV4GPSModule.longitudeEW=(GPS_coord[LON]<0);
+ deg = GPS_coord[LON] / 100000;
+ sec = (GPS_coord[LON] - (deg * 100000)) * 6;
+ min = sec / 10000;
+ sec = sec % 10000;
+ degMin = (deg * 100) + min;
+ HoTTV4GPSModule.longitudeMinLow = degMin;
+ HoTTV4GPSModule.longitudeMinHigh = degMin >> 8;
+ HoTTV4GPSModule.longitudeSecLow = sec;
+ HoTTV4GPSModule.longitudeSecHigh = sec >> 8;
+
+ // GPS Speed in km/h
+ uint16_t speed = (GPS_speed / 100) * 36; // 0.1m/s * 0.36 = km/h
+ HoTTV4GPSModule.GPSSpeedLow = speed & 0x00FF;
+ HoTTV4GPSModule.GPSSpeedHigh = speed >> 8;
+
+ // Distance to home
+ HoTTV4GPSModule.distanceLow = GPS_distanceToHome & 0x00FF;
+ HoTTV4GPSModule.distanceHigh = GPS_distanceToHome >> 8;
+
+ // Altitude
+ HoTTV4GPSModule.altitudeLow = GPS_altitude & 0x00FF;
+ HoTTV4GPSModule.altitudeHigh = GPS_altitude >> 8;
+
+ // Direction to home
+ HoTTV4GPSModule.HomeDirection = GPS_directionToHome;
+ } else {
+ HoTTV4GPSModule.GPS_fix = 0x20; // Displays a ' ' to show nothing or clear the old value
+ }
+}
+
+/**
+ * Writes cell 1-4 high, low values and if not available
+ * calculates vbat.
+ */
+static void hottV4EAMUpdateBattery(void)
+{
+#if 0
+ HoTTV4ElectricAirModule.cell1L = 4.2f * 10 * 5; // 2mv step
+ HoTTV4ElectricAirModule.cell1H = 0;
+
+ HoTTV4ElectricAirModule.cell2L = 0;
+ HoTTV4ElectricAirModule.cell2H = 0;
+
+ HoTTV4ElectricAirModule.cell3L = 0;
+ HoTTV4ElectricAirModule.cell3H = 0;
+
+ HoTTV4ElectricAirModule.cell4L = 0;
+ HoTTV4ElectricAirModule.cell4H = 0;
+#endif
+
+ HoTTV4ElectricAirModule.driveVoltageLow = vbat & 0xFF;
+ HoTTV4ElectricAirModule.driveVoltageHigh = vbat >> 8;
+ HoTTV4ElectricAirModule.battery1Low = vbat & 0xFF;
+ HoTTV4ElectricAirModule.battery1High = vbat >> 8;
+
+#if 0
+ HoTTV4ElectricAirModule.battery2Low = 0 & 0xFF;
+ HoTTV4ElectricAirModule.battery2High = 0 >> 8;
+
+ if (batteryWarning) {
+ HoTTV4ElectricAirModule.alarmTone = HoTTv4NotificationUndervoltage;
+ HoTTV4ElectricAirModule.alarmInverse1 |= 0x80; // Invert Voltage display
+ }
+#endif
+}
+
+static void hottV4EAMUpdateTemperatures(void)
+{
+ HoTTV4ElectricAirModule.temp1 = 20 + 0;
+ HoTTV4ElectricAirModule.temp2 = 20;
+
+#if 0
+ if (HoTTV4ElectricAirModule.temp1 >= (20 + MultiHoTTModuleSettings.alarmTemp1)) {
+ HoTTV4ElectricAirModule.alarmTone = HoTTv4NotificationMaxTemperature;
+ HoTTV4ElectricAirModule.alarmInverse |= 0x8; // Invert Temp1 display
+ }
+#endif
+}
+
+
+/**
+ * Sends HoTTv4 capable EAM telemetry frame.
+ */
+void hottV4FormatAndSendEAMResponse(void)
+{
+ memset(&HoTTV4ElectricAirModule, 0, sizeof(HoTTV4ElectricAirModule));
+
+ // Minimum data set for EAM
+ HoTTV4ElectricAirModule.startByte = 0x7C;
+ HoTTV4ElectricAirModule.sensorID = HOTTV4_ELECTRICAL_AIR_SENSOR_ID;
+ HoTTV4ElectricAirModule.sensorTextID = HOTTV4_ELECTRICAL_AIR_SENSOR_TEXT_ID;
+ HoTTV4ElectricAirModule.endByte = 0x7D;
+
+ // Reset alarms
+ HoTTV4ElectricAirModule.alarmTone = 0x0;
+ HoTTV4ElectricAirModule.alarmInverse1 = 0x0;
+
+ hottV4EAMUpdateBattery();
+ hottV4EAMUpdateTemperatures();
+
+ HoTTV4ElectricAirModule.current = 0 / 10;
+ HoTTV4ElectricAirModule.height = OFFSET_HEIGHT + 0;
+ HoTTV4ElectricAirModule.m2s = OFFSET_M2S;
+ HoTTV4ElectricAirModule.m3s = OFFSET_M3S;
+
+ hottV4Respond((uint8_t*)&HoTTV4ElectricAirModule, sizeof(HoTTV4ElectricAirModule));
+}
+
+static void hottV4Respond(uint8_t *data, uint8_t size)
+{
+ serialSetMode(core.telemport, MODE_TX);
+
+ uint16_t crc = 0;
+ uint8_t i;
+
+ for (i = 0; i < size - 1; i++) {
+ crc += data[i];
+ hottV4SerialWrite(data[i]);
+
+ // Protocol specific delay between each transmitted byte
+ delayMicroseconds(HOTTV4_TX_DELAY);
+ }
+
+ hottV4SerialWrite(crc & 0xFF);
+
+ delayMicroseconds(HOTTV4_TX_DELAY);
+
+ serialSetMode(core.telemport, MODE_RX);
+}
+
+static void hottV4SerialWrite(uint8_t c)
+{
+ serialWrite(core.telemport, c);
+}
+
+void configureHoTTTelemetryPort(void)
+{
+ // TODO set speed here to 19200?
+ serialSetMode(core.telemport, MODE_RX);
+}
+
+void freeHoTTTelemetryPort(void)
+{
+ serialSetMode(core.telemport, MODE_RXTX);
+}
+
+void handleHoTTTelemetry(void)
+{
+ uint8_t c;
+
+ while (serialTotalBytesWaiting(core.telemport) > 0) {
+ c = serialRead(core.telemport);
+
+ // Protocol specific waiting time to avoid collisions
+ delay(5);
+
+ switch (c) {
+ case 0x8A:
+ if (sensors(SENSOR_GPS)) hottV4FormatAndSendGPSResponse();
+ break;
+ case 0x8E:
+ hottV4FormatAndSendEAMResponse();
+ break;
+ }
+ }
+}
+
diff --git a/src/telemetry_hott.h b/src/telemetry_hott.h
new file mode 100644
index 0000000000..f808defecf
--- /dev/null
+++ b/src/telemetry_hott.h
@@ -0,0 +1,209 @@
+/*
+ * telemetry_hott.h
+ *
+ * Created on: 6 Apr 2014
+ * Author: Hydra
+ */
+
+#ifndef TELEMETRY_HOTT_H_
+#define TELEMETRY_HOTT_H_
+
+/* HoTT module specifications */
+
+#define HOTTV4_GENERAL_AIR_SENSOR_ID 0xD0
+
+#define HOTTV4_ELECTRICAL_AIR_SENSOR_ID 0x8E // Electric Air Sensor ID
+#define HOTTV4_ELECTRICAL_AIR_SENSOR_TEXT_ID 0xE0 // Electric Air Module ID
+
+#define HOTTV4_GPS_SENSOR_ID 0x8A // GPS Sensor ID
+#define HOTTV4_GPS_SENSOR_TEXT_ID 0xA0 // GPS Module ID
+
+#define HOTTV4_RXTX 4
+#define HOTTV4_TX_DELAY 1000
+
+#define HOTTV4_BUTTON_DEC 0xEB
+#define HOTTV4_BUTTON_INC 0xED
+#define HOTTV4_BUTTON_SET 0xE9
+#define HOTTV4_BUTTON_NIL 0x0F
+#define HOTTV4_BUTTON_NEXT 0xEE
+#define HOTTV4_BUTTON_PREV 0xE7
+
+#define OFFSET_HEIGHT 500
+#define OFFSET_M2S 72
+#define OFFSET_M3S 120
+
+typedef enum {
+ HoTTv4NotificationErrorCalibration = 0x01,
+ HoTTv4NotificationErrorReceiver = 0x02,
+ HoTTv4NotificationErrorDataBus = 0x03,
+ HoTTv4NotificationErrorNavigation = 0x04,
+ HoTTv4NotificationErrorError = 0x05,
+ HoTTv4NotificationErrorCompass = 0x06,
+ HoTTv4NotificationErrorSensor = 0x07,
+ HoTTv4NotificationErrorGPS = 0x08,
+ HoTTv4NotificationErrorMotor = 0x09,
+
+ HoTTv4NotificationMaxTemperature = 0x0A,
+ HoTTv4NotificationAltitudeReached = 0x0B,
+ HoTTv4NotificationWaypointReached = 0x0C,
+ HoTTv4NotificationNextWaypoint = 0x0D,
+ HoTTv4NotificationLanding = 0x0E,
+ HoTTv4NotificationGPSFix = 0x0F,
+ HoTTv4NotificationUndervoltage = 0x10,
+ HoTTv4NotificationGPSHold = 0x11,
+ HoTTv4NotificationGPSHome = 0x12,
+ HoTTv4NotificationGPSOff = 0x13,
+ HoTTv4NotificationBeep = 0x14,
+ HoTTv4NotificationMicrocopter = 0x15,
+ HoTTv4NotificationCapacity = 0x16,
+ HoTTv4NotificationCareFreeOff = 0x17,
+ HoTTv4NotificationCalibrating = 0x18,
+ HoTTv4NotificationMaxRange = 0x19,
+ HoTTv4NotificationMaxAltitude = 0x1A,
+
+ HoTTv4Notification20Meter = 0x25,
+ HoTTv4NotificationMicrocopterOff = 0x26,
+ HoTTv4NotificationAltitudeOn = 0x27,
+ HoTTv4NotificationAltitudeOff = 0x28,
+ HoTTv4Notification100Meter = 0x29,
+ HoTTv4NotificationCareFreeOn = 0x2E,
+ HoTTv4NotificationDown = 0x2F,
+ HoTTv4NotificationUp = 0x30,
+ HoTTv4NotificationHold = 0x31,
+ HoTTv4NotificationGPSOn = 0x32,
+ HoTTv4NotificationFollowing = 0x33,
+ HoTTv4NotificationStarting = 0x34,
+ HoTTv4NotificationReceiver = 0x35,
+} HoTTv4Notification;
+
+/**
+ * GPS
+ * Receiver -> GPS Sensor (Flightcontrol)
+ * Byte 1: 0x80 = Receiver byte
+ * Byte 2: 0x8A = GPS Sensor byte (witch data Transmitter wants to get)
+ * 5ms Idle Line!
+ * 5ms delay
+ */
+
+struct {
+ uint8_t startByte; // Byte 1: 0x7C = Start byte data
+ uint8_t sensorID; // Byte 2: 0x8A = GPS Sensor
+ uint8_t alarmTone; // Byte 3: 0…= warning beeps
+ uint8_t sensorTextID; // Byte 4: 160 0xA0 Sensor ID Neu!
+ uint8_t alarmInverse1; // Byte 5: 01 inverse status
+ uint8_t alarmInverse2; // Byte 6: 00 inverse status status 1 = no GPS signal
+ uint8_t flightDirection; // Byte 7: 119 = fly direction. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West)
+ uint8_t GPSSpeedLow; // Byte 8: 8 = GPS speed low byte 8km/h
+ uint8_t GPSSpeedHigh; // Byte 9: 0 = GPS speed high byte
+
+ // Example: N 48°39'988"
+ uint8_t LatitudeNS; // Byte 10: 000 = N; 001 = S
+ // 0x12E7 = 4839 (48°30')
+ uint8_t LatitudeMinLow; // Byte 11: 231 = 0xE7
+ uint8_t LatitudeMinHigh; // Byte 12: 018 = 0x12
+ // 0x03DC = 0988 (988")
+ uint8_t LatitudeSecLow; // Byte 13: 220 = 0xDC
+ uint8_t LatitudeSecHigh; // Byte 14: 003 = 0x03
+
+ // Example: E 9°25'9360"
+ uint8_t longitudeEW; // Byte 15: 000 = E; 001 = W;
+ // 0x039D = 0925 (09°25')
+ uint8_t longitudeMinLow; // Byte 16: 157 = 0x9D
+ uint8_t longitudeMinHigh; // Byte 17: 003 = 0x03
+ // 0x2490 = 9360 (9360")
+ uint8_t longitudeSecLow; // Byte 18: 144 = 0x90
+ uint8_t longitudeSecHigh; // Byte 19: 036 = 0x24
+
+ uint8_t distanceLow; // Byte 20: distance low byte (meter)
+ uint8_t distanceHigh; // Byte 21: distance high byte
+ uint8_t altitudeLow; // Byte 22: Altitude low byte (meter)
+ uint8_t altitudeHigh; // Byte 23: Altitude high byte
+ uint8_t resolutionLow; // Byte 24: Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s)
+ uint8_t resolutionHigh; // Byte 25: High Byte m/s resolution 0.01m
+
+ uint8_t m3s; // Byte 26: 120 = 0m/3s
+ uint8_t GPSNumSat; // Byte 27: number of satelites (1 byte)
+ uint8_t GPSFixChar; // Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte)
+ uint8_t HomeDirection; // Byte 29: HomeDirection (direction from starting point to Model position) (1 byte)
+ uint8_t angleXdirection; // Byte 30: angle x-direction (1 byte)
+ uint8_t angleYdirection; // Byte 31: angle y-direction (1 byte)
+ uint8_t angleZdirection; // Byte 32: angle z-direction (1 byte)
+
+ uint8_t gyroXLow; // Byte 33: gyro x low byte (2 bytes)
+ uint8_t gyroXHigh; // Byte 34: gyro x high byte
+ uint8_t gyroYLow; // Byte 35: gyro y low byte (2 bytes)
+ uint8_t gyroYHigh; // Byte 36: gyro y high byte
+ uint8_t gyroZLow; // Byte 37: gyro z low byte (2 bytes)
+ uint8_t gyroZHigh; // Byte 38: gyro z high byte
+
+ uint8_t vibration; // Byte 39: vibration (1 bytes)
+ uint8_t Ascii4; // Byte 40: 00 ASCII Free Character [4]
+ uint8_t Ascii5; // Byte 41: 00 ASCII Free Character [5]
+ uint8_t GPS_fix; // Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX
+ uint8_t version; // Byte 43: 00 version number
+ uint8_t endByte; // Byte 44: 0x7D End byte
+} HoTTV4GPSModule;
+
+/**
+ * EAM (Electric Air Module) 33620
+ * EmpfängerElectric Sensor
+ * Byte 1: 80 = Receiver byte
+ * Byte 2: 8E = Electric Sensor byte
+ * 5ms Idle Line!
+*/
+
+struct {
+ uint8_t startByte;
+ uint8_t sensorID;
+ uint8_t alarmTone; // Alarm */
+ uint8_t sensorTextID;
+ uint8_t alarmInverse1;
+ uint8_t alarmInverse2;
+
+ uint8_t cell1L; // Low Voltage Cell 1-7 in 2mV steps */
+ uint8_t cell2L;
+ uint8_t cell3L;
+ uint8_t cell4L;
+ uint8_t cell5L;
+ uint8_t cell6L;
+ uint8_t cell7L;
+ uint8_t cell1H; // High Voltage Cell 1-7 in 2mV steps */
+ uint8_t cell2H;
+ uint8_t cell3H;
+ uint8_t cell4H;
+ uint8_t cell5H;
+ uint8_t cell6H;
+ uint8_t cell7H;
+
+ uint8_t battery1Low; // Battetry 1 LSB/MSB in 100mv steps; 50 == 5V */
+ uint8_t battery1High; // Battetry 1 LSB/MSB in 100mv steps; 50 == 5V */
+ uint8_t battery2Low; // Battetry 2 LSB/MSB in 100mv steps; 50 == 5V */
+ uint8_t battery2High; // Battetry 2 LSB/MSB in 100mv steps; 50 == 5V */
+
+ uint8_t temp1; // Temp 1; Offset of 20. 20 == 0C */
+ uint8_t temp2; // Temp 2; Offset of 20. 20 == 0C */
+
+ uint16_t height; // Height. Offset -500. 500 == 0 */
+ uint16_t current; // 1 = 0.1A */
+ uint8_t driveVoltageLow;
+ uint8_t driveVoltageHigh;
+ uint16_t capacity; // mAh */
+ uint16_t m2s; // m2s; 0x48 == 0 */
+ uint8_t m3s; // m3s; 0x78 == 0 */
+
+ uint16_t rpm; // RPM. 10er steps; 300 == 3000rpm */
+ uint8_t minutes;
+ uint8_t seconds;
+ uint8_t speed;
+
+ uint8_t version;
+ uint8_t endByte;
+} HoTTV4ElectricAirModule;
+
+void handleHoTTTelemetry(void);
+void checkTelemetryState(void);
+
+void configureHoTTTelemetryPort(void);
+void freeHoTTTelemetryPort(void);
+
+#endif /* TELEMETRY_HOTT_H_ */