added smartport telemetry

Conflicts:

	src/main/telemetry/telemetry.h

Makefile

@@ -210,6 +210,7 @@ HIGHEND_SRC  = flight/autotune.c \
 		   telemetry/frsky.c \
 		   telemetry/hott.c \
 		   telemetry/msp.c \
+           telemetry/smartport.c \
 		   sensors/sonar.c \
 		   sensors/barometer.c
 

src/main/config/config.c

@@ -203,7 +203,7 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
 void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
 {
     telemetryConfig->telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
-    telemetryConfig->frsky_inversion = SERIAL_NOT_INVERTED;
+    telemetryConfig->telemetry_inversion = SERIAL_NOT_INVERTED;
     telemetryConfig->telemetry_switch = 0;
     telemetryConfig->gpsNoFixLatitude = 0;
     telemetryConfig->gpsNoFixLongitude = 0;

src/main/io/serial.c

@@ -38,7 +38,10 @@
 
 #include "config/config.h"
 
-uint32_t getTelemetryProviderBaudRate(void);
+#ifdef TELEMETRY
+#include "telemetry/telemetry.h"
+#endif
+
 void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate);
 
 void mspInit(serialConfig_t *serialConfig);
@@ -122,12 +125,13 @@ static const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
 #endif
 
 const functionConstraint_t functionConstraints[] = {
-        { FUNCTION_CLI,             9600, 115200, NO_AUTOBAUD, SPF_NONE },
+        { FUNCTION_CLI,                 9600,  115200, NO_AUTOBAUD, SPF_NONE },
-        { FUNCTION_GPS,             9600, 115200, AUTOBAUD,    SPF_NONE },
+        { FUNCTION_GPS,                 9600,  115200, AUTOBAUD,    SPF_NONE },
-        { FUNCTION_GPS_PASSTHROUGH, 9600, 115200, NO_AUTOBAUD, SPF_NONE },
+        { FUNCTION_GPS_PASSTHROUGH,     9600,  115200, NO_AUTOBAUD, SPF_NONE },
-        { FUNCTION_MSP,             9600, 115200, NO_AUTOBAUD, SPF_NONE },
+        { FUNCTION_MSP,                 9600,  115200, NO_AUTOBAUD, SPF_NONE },
-        { FUNCTION_SERIAL_RX,       9600, 115200, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK },
+        { FUNCTION_SERIAL_RX,           9600,  115200, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK },
-        { FUNCTION_TELEMETRY,       9600, 19200,  NO_AUTOBAUD, SPF_NONE }
+        { FUNCTION_TELEMETRY,           9600,  19200,  NO_AUTOBAUD, SPF_NONE },
+        { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600,  NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE }
 };
 
 #define FUNCTION_CONSTRAINT_COUNT (sizeof(functionConstraints) / sizeof(functionConstraint_t))
@@ -403,6 +407,7 @@ functionConstraint_t *getConfiguredFunctionConstraint(serialPortFunction_e funct
 
 #ifdef TELEMETRY
         case FUNCTION_TELEMETRY:
+        case FUNCTION_SMARTPORT_TELEMETRY:
             configuredFunctionConstraint.minBaudRate = getTelemetryProviderBaudRate();
             configuredFunctionConstraint.maxBaudRate = configuredFunctionConstraint.minBaudRate;
             break;
@@ -454,6 +459,11 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
 
     functionConstraint = getConfiguredFunctionConstraint(FUNCTION_TELEMETRY);
     searchResult = findSerialPort(FUNCTION_TELEMETRY, functionConstraint);
+    // TODO check explicitly for SmartPort config
+    if (!searchResult) {
+        functionConstraint = getConfiguredFunctionConstraint(FUNCTION_SMARTPORT_TELEMETRY);
+        searchResult = findSerialPort(FUNCTION_SMARTPORT_TELEMETRY, functionConstraint);
+    }
     if (feature(FEATURE_TELEMETRY) && !searchResult) {
         return false;
     }
@@ -627,8 +637,15 @@ void serialInit(serialConfig_t *initialSerialConfig)
     serialConfig = initialSerialConfig;
     applySerialConfigToPortFunctions(serialConfig);
 
-    mspInit(serialConfig);
+#ifdef TELEMETRY
-    cliInit(serialConfig);
+    if (telemetryAllowsOtherSerial(FUNCTION_MSP))
+#endif
+        mspInit(serialConfig);
+
+#ifdef TELEMETRY
+    if (telemetryAllowsOtherSerial(FUNCTION_CLI))
+#endif
+        cliInit(serialConfig);
 }
 
 void handleSerial(void)
@@ -639,7 +656,10 @@ void handleSerial(void)
         return;
     }
 
-    mspProcess();
+#ifdef TELEMETRY
+    if (telemetryAllowsOtherSerial(FUNCTION_MSP))
+#endif
+        mspProcess();
 }
 
 void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)

src/main/io/serial.h

@@ -18,13 +18,14 @@
 #pragma once
 
 typedef enum {
-    FUNCTION_NONE               = 0,
+    FUNCTION_NONE                = 0,
-    FUNCTION_MSP                = (1 << 0),
+    FUNCTION_MSP                 = (1 << 0),
-    FUNCTION_CLI                = (1 << 1),
+    FUNCTION_CLI                 = (1 << 1),
-    FUNCTION_TELEMETRY          = (1 << 2),
+    FUNCTION_TELEMETRY           = (1 << 2),
-    FUNCTION_SERIAL_RX          = (1 << 3),
+    FUNCTION_SMARTPORT_TELEMETRY = (1 << 3),
-    FUNCTION_GPS                = (1 << 4),
+    FUNCTION_SERIAL_RX           = (1 << 4),
-    FUNCTION_GPS_PASSTHROUGH    = (1 << 5)
+    FUNCTION_GPS                 = (1 << 5),
+    FUNCTION_GPS_PASSTHROUGH     = (1 << 6)
 } serialPortFunction_e;
 
 typedef enum {
@@ -48,9 +49,10 @@ typedef enum {
     SCENARIO_GPS_PASSTHROUGH_ONLY               = FUNCTION_GPS_PASSTHROUGH,
     SCENARIO_MSP_ONLY                           = FUNCTION_MSP,
     SCENARIO_MSP_CLI_GPS_PASTHROUGH             = FUNCTION_CLI | FUNCTION_MSP | FUNCTION_GPS_PASSTHROUGH,
-    SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH   = FUNCTION_MSP | FUNCTION_CLI | FUNCTION_TELEMETRY | FUNCTION_GPS_PASSTHROUGH,
+    SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH   = FUNCTION_MSP | FUNCTION_CLI | FUNCTION_TELEMETRY | FUNCTION_SMARTPORT_TELEMETRY | FUNCTION_GPS_PASSTHROUGH,
     SCENARIO_SERIAL_RX_ONLY                     = FUNCTION_SERIAL_RX,
     SCENARIO_TELEMETRY_ONLY                     = FUNCTION_TELEMETRY,
+    SCENARIO_SMARTPORT_TELEMETRY_ONLY           = FUNCTION_SMARTPORT_TELEMETRY
 } serialPortFunctionScenario_e;
 
 #define SERIAL_PORT_SCENARIO_COUNT 9

src/main/io/serial_cli.c

@@ -273,7 +273,7 @@ const clivalue_t valueTable[] = {
 
     { "telemetry_provider",         VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
     { "telemetry_switch",           VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.telemetry_switch, 0, 1 },
-    { "frsky_inversion",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.frsky_inversion, 0, 1 },
+    { "telemetry_inversion",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.telemetry_inversion, 0, 1 },
     { "frsky_default_lattitude",    VAR_FLOAT  | MASTER_VALUE,  &masterConfig.telemetryConfig.gpsNoFixLatitude, -90.0, 90.0 },
     { "frsky_default_longitude",    VAR_FLOAT  | MASTER_VALUE,  &masterConfig.telemetryConfig.gpsNoFixLongitude, -180.0, 180.0 },
     { "frsky_coordinates_format",   VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.frsky_coordinate_format, 0, FRSKY_FORMAT_NMEA },

src/main/telemetry/frsky.c

@@ -420,7 +420,7 @@ void configureFrSkyTelemetryPort(void)
         serialSetMode(frskyPort, FRSKY_INITIAL_PORT_MODE);
         beginSerialPortFunction(frskyPort, FUNCTION_TELEMETRY);
     } else {
-        frskyPort = openSerialPort(FUNCTION_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->frsky_inversion);
+        frskyPort = openSerialPort(FUNCTION_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion);
 
         // FIXME only need these values to reset the port if the port is shared
         previousPortMode = frskyPort->mode;

src/main/telemetry/smartport.c

@@ -0,0 +1,470 @@
+/*
+ * SmartPort Telemetry implementation by frank26080115
+ * see https://github.com/frank26080115/cleanflight/wiki/Using-Smart-Port
+ */
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef TELEMETRY
+
+#include "common/axis.h"
+#include "common/color.h"
+#include "common/maths.h"
+
+#include "drivers/system.h"
+#include "drivers/accgyro.h"
+#include "drivers/serial.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/gpio.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_rx.h"
+#include "drivers/adc.h"
+#include "drivers/light_led.h"
+
+#include "flight/flight.h"
+#include "flight/mixer.h"
+#include "flight/failsafe.h"
+#include "flight/navigation.h"
+#include "rx/rx.h"
+#include "rx/msp.h"
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+#include "io/gps.h"
+#include "io/gimbal.h"
+#include "io/serial.h"
+#include "io/ledstrip.h"
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+#include "sensors/battery.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/compass.h"
+#include "sensors/gyro.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/smartport.h"
+
+#include "config/runtime_config.h"
+#include "config/config.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+
+enum
+{
+    SPSTATE_UNINITIALIZED,
+    SPSTATE_INITIALIZED,
+    SPSTATE_WORKING,
+    SPSTATE_TIMEDOUT,
+    SPSTATE_DEINITIALIZED,
+};
+
+enum
+{
+    FSSP_START_STOP = 0x7E,
+    FSSP_DATA_FRAME = 0x10,
+
+    // ID of sensor. Must be something that is polled by FrSky RX
+    FSSP_SENSOR_ID1 = 0x1B,
+    FSSP_SENSOR_ID2 = 0x0D,
+    FSSP_SENSOR_ID3 = 0x34,
+    FSSP_SENSOR_ID4 = 0x67,
+    // reverse engineering tells me that there are plenty more IDs
+};
+
+// these data identifiers are obtained from http://diydrones.com/forum/topics/amp-to-frsky-x8r-sport-converter
+enum
+{
+    FSSP_DATAID_SPEED      = 0x0830 ,
+    FSSP_DATAID_VFAS       = 0x0210 ,
+    FSSP_DATAID_CURRENT    = 0x0200 ,
+    FSSP_DATAID_RPM        = 0x050F ,
+    FSSP_DATAID_ALTITUDE   = 0x0100 ,
+    FSSP_DATAID_FUEL       = 0x0600 ,
+    FSSP_DATAID_ADC1       = 0xF102 ,
+    FSSP_DATAID_ADC2       = 0xF103 ,
+    FSSP_DATAID_LATLONG    = 0x0800 ,
+    FSSP_DATAID_CAP_USED   = 0x0600 ,
+    FSSP_DATAID_VARIO      = 0x0110 ,
+    FSSP_DATAID_CELLS      = 0x0300 ,
+    FSSP_DATAID_CELLS_LAST = 0x030F ,
+    FSSP_DATAID_HEADING    = 0x0840 ,
+    FSSP_DATAID_ACCX       = 0x0700 ,
+    FSSP_DATAID_ACCY       = 0x0710 ,
+    FSSP_DATAID_ACCZ       = 0x0720 ,
+    FSSP_DATAID_T1         = 0x0400 ,
+    FSSP_DATAID_T2         = 0x0410 ,
+    FSSP_DATAID_GPS_ALT    = 0x0820 ,
+};
+
+const uint16_t frSkyDataIdTable[] = {
+    FSSP_DATAID_SPEED     ,
+    FSSP_DATAID_VFAS      ,
+    FSSP_DATAID_CURRENT   ,
+    //FSSP_DATAID_RPM       ,
+    FSSP_DATAID_ALTITUDE  ,
+    FSSP_DATAID_FUEL      ,
+    //FSSP_DATAID_ADC1      ,
+    //FSSP_DATAID_ADC2      ,
+    FSSP_DATAID_LATLONG   ,
+    FSSP_DATAID_LATLONG   , // twice
+    //FSSP_DATAID_CAP_USED  ,
+    FSSP_DATAID_VARIO     ,
+    //FSSP_DATAID_CELLS     ,
+    //FSSP_DATAID_CELLS_LAST,
+    FSSP_DATAID_HEADING   ,
+    FSSP_DATAID_ACCX      ,
+    FSSP_DATAID_ACCY      ,
+    FSSP_DATAID_ACCZ      ,
+    FSSP_DATAID_T1        ,
+    FSSP_DATAID_T2        ,
+    FSSP_DATAID_GPS_ALT   ,
+    0
+};
+
+#define __USE_C99_MATH // for roundf()
+#define SMARTPORT_BAUD 57600
+#define SMARTPORT_UART_MODE MODE_BIDIR
+#define SMARTPORT_SERVICE_DELAY_MS 5 // telemetry requests comes in at roughly 12 ms intervals, keep this under that
+#define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000
+
+static serialPort_t *mySerPort;
+static telemetryConfig_t *telemetryConfig;
+static portMode_t previousPortMode;
+static uint32_t previousBaudRate;
+extern void serialInit(serialConfig_t *); // from main.c
+
+char smartPortState = SPSTATE_UNINITIALIZED;
+static uint8_t smartPortHasRequest = 0;
+static uint8_t smartPortIdCnt = 0;
+static uint32_t smartPortLastRequestTime = 0;
+static uint32_t smartPortLastServiceTime = 0;
+
+static void smartPortDataReceive(uint16_t c)
+{
+    uint32_t now = millis();
+
+    // look for a valid request sequence
+    static uint8_t lastChar;
+    if (lastChar == FSSP_START_STOP) {
+        smartPortState = SPSTATE_WORKING;
+        smartPortLastRequestTime = now;
+        if ((c == FSSP_SENSOR_ID1) ||
+            (c == FSSP_SENSOR_ID2) ||
+            (c == FSSP_SENSOR_ID3) ||
+            (c == FSSP_SENSOR_ID4)) {
+            smartPortHasRequest = 1;
+            // we only responde to these IDs
+            // the X4R-SB does send other IDs, we ignore them, but take note of the time
+        }
+    }
+    lastChar = c;
+}
+
+static void smartPortSendByte(uint8_t c, uint16_t *crcp)
+{
+    // smart port escape sequence
+    if (c == 0x7D || c == 0x7E) {
+        serialWrite(mySerPort, 0x7D);
+        c ^= 0x20;
+    }
+
+    serialWrite(mySerPort, c);
+
+    if (crcp == NULL)
+        return;
+
+    uint16_t crc = *crcp;
+    crc += c;
+    crc += crc >> 8;
+    crc &= 0x00FF;
+    crc += crc >> 8;
+    crc &= 0x00FF;
+    *crcp = crc;
+}
+
+static void smartPortSendPackage(uint16_t id, uint32_t val)
+{
+    uint16_t crc = 0;
+    smartPortSendByte(FSSP_DATA_FRAME, &crc);
+    uint8_t *u8p = (uint8_t*)&id;
+    smartPortSendByte(u8p[0], &crc);
+    smartPortSendByte(u8p[1], &crc);
+    u8p = (uint8_t*)&val;
+    smartPortSendByte(u8p[0], &crc);
+    smartPortSendByte(u8p[1], &crc);
+    smartPortSendByte(u8p[2], &crc);
+    smartPortSendByte(u8p[3], &crc);
+    smartPortSendByte(0xFF - (uint8_t)crc, NULL);
+
+    smartPortLastServiceTime = millis();
+}
+
+void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig)
+{
+    telemetryConfig = initialTelemetryConfig;
+}
+
+void freeSmartPortTelemetryPort(void)
+{
+    if (smartPortState == SPSTATE_UNINITIALIZED)
+        return;
+    if (smartPortState == SPSTATE_DEINITIALIZED)
+        return;
+
+    if (isTelemetryPortShared()) {
+        endSerialPortFunction(mySerPort, FUNCTION_SMARTPORT_TELEMETRY);
+        smartPortState = SPSTATE_DEINITIALIZED;
+        serialInit(&masterConfig.serialConfig);
+    }
+    else {
+        serialSetMode(mySerPort, previousPortMode);
+        serialSetBaudRate(mySerPort, previousBaudRate);
+        endSerialPortFunction(mySerPort, FUNCTION_SMARTPORT_TELEMETRY);
+        smartPortState = SPSTATE_DEINITIALIZED;
+    }
+    mySerPort = NULL;
+}
+
+void configureSmartPortTelemetryPort(void)
+{
+    if (smartPortState != SPSTATE_UNINITIALIZED) {
+        // do not allow reinitialization
+        return;
+    }
+
+    mySerPort = findOpenSerialPort(FUNCTION_SMARTPORT_TELEMETRY);
+    if (mySerPort) {
+        previousPortMode = mySerPort->mode;
+        previousBaudRate = mySerPort->baudRate;
+
+        //waitForSerialPortToFinishTransmitting(mySerPort); // FIXME locks up the system
+
+        serialSetBaudRate(mySerPort, SMARTPORT_BAUD);
+        serialSetMode(mySerPort, SMARTPORT_UART_MODE);
+        beginSerialPortFunction(mySerPort, FUNCTION_SMARTPORT_TELEMETRY);
+    } else {
+        mySerPort = openSerialPort(FUNCTION_SMARTPORT_TELEMETRY, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, telemetryConfig->telemetry_inversion);
+
+        if (mySerPort) {
+            smartPortState = SPSTATE_INITIALIZED;
+            previousPortMode = mySerPort->mode;
+            previousBaudRate = mySerPort->baudRate;
+        }
+        else {
+            // failed, resume MSP and CLI
+            if (isTelemetryPortShared()) {
+                smartPortState = SPSTATE_DEINITIALIZED;
+                serialInit(&masterConfig.serialConfig);
+            }
+        }
+    }
+}
+
+bool canSendSmartPortTelemetry(void)
+{
+    return smartPortState == SPSTATE_INITIALIZED || smartPortState == SPSTATE_WORKING;
+}
+
+bool canSmartPortAllowOtherSerial(void)
+{
+    return smartPortState == SPSTATE_DEINITIALIZED;
+}
+
+bool isSmartPortTimedOut(void)
+{
+    return smartPortState >= SPSTATE_TIMEDOUT;
+}
+
+uint32_t getSmartPortTelemetryProviderBaudRate(void)
+{
+    return SMARTPORT_BAUD;
+}
+
+void handleSmartPortTelemetry(void)
+{
+    if (!canSendSmartPortTelemetry())
+        return;
+
+    while (serialTotalBytesWaiting(mySerPort) > 0) {
+        uint8_t c = serialRead(mySerPort);
+        smartPortDataReceive(c);
+    }
+
+    uint32_t now = millis();
+
+    // if timed out, reconfigure the UART back to normal so the GUI or CLI works
+    if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) {
+        smartPortState = SPSTATE_TIMEDOUT;
+        return;
+    }
+
+    // limit the rate at which we send responses, we don't want to affect flight characteristics (but USART1 is using DMA so I doubt it matters)
+    if ((now - smartPortLastServiceTime) < SMARTPORT_SERVICE_DELAY_MS)
+        return;
+
+    if (smartPortHasRequest) {
+        // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
+        uint16_t id = frSkyDataIdTable[smartPortIdCnt];
+        if (id == 0) { // end of table reached, loop back
+            smartPortIdCnt = 0;
+            id = frSkyDataIdTable[smartPortIdCnt];
+        }
+        smartPortIdCnt++;
+
+        float tmpf;
+        int32_t tmpi;
+        uint32_t tmpui;
+        static uint8_t t1Cnt = 0;
+
+        switch(id) {
+            case FSSP_DATAID_SPEED      :
+                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
+                    tmpf = GPS_speed;
+                    tmpf *= 0.36;
+                    smartPortSendPackage(id, (uint32_t)lroundf(tmpf)); // given in 0.1 m/s, provide in KM/H
+                    smartPortHasRequest = 0;
+                }
+                break;
+            case FSSP_DATAID_VFAS       :
+                smartPortSendPackage(id, vbat * 83); // supposedly given in 0.1V, unknown requested unit
+                // multiplying by 83 seems to make Taranis read correctly
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_CURRENT    :
+                smartPortSendPackage(id, amperage); // given in 10mA steps, unknown requested unit
+                smartPortHasRequest = 0;
+                break;
+            //case FSSP_DATAID_RPM        :
+            case FSSP_DATAID_ALTITUDE   :
+                smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_FUEL       :
+                smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
+                smartPortHasRequest = 0;
+                break;
+            //case FSSP_DATAID_ADC1       :
+            //case FSSP_DATAID_ADC2       :
+            case FSSP_DATAID_LATLONG    :
+                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
+                    tmpui = 0;
+                    // the same ID is sent twice, one for longitude, one for latitude
+                    // the MSB of the sent uint32_t helps FrSky keep track
+                    // the even/odd bit of our counter helps us keep track                    
+                    if (smartPortIdCnt & 1) {
+                        tmpui = tmpi = GPS_coord[LON];
+                        if (tmpi < 0) {
+                            tmpui = -tmpi;
+                            tmpui |= 0x40000000;
+                        }
+                        tmpui |= 0x80000000;
+                    }
+                    else {
+                        tmpui = tmpi = GPS_coord[LAT];
+                        if (tmpi < 0) {
+                            tmpui = -tmpi;
+                            tmpui |= 0x40000000;
+                        }
+                    }
+                    smartPortSendPackage(id, tmpui);
+                    smartPortHasRequest = 0;
+                }
+                break;
+            //case FSSP_DATAID_CAP_USED   :
+            case FSSP_DATAID_VARIO      :
+                smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_HEADING    :
+                smartPortSendPackage(id, heading / 10); // given in 0.1 deg, requested in 10000 = 100 deg
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_ACCX       :
+                smartPortSendPackage(id, accSmooth[X] / 44);
+                // unknown input and unknown output unit
+                // we can only show 00.00 format, another digit won't display right on Taranis
+                // dividing by roughly 44 will give acceleration in G units
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_ACCY       :
+                smartPortSendPackage(id, accSmooth[Y] / 44);
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_ACCZ       :
+                smartPortSendPackage(id, accSmooth[Z] / 44);
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_T1         :
+                // we send all the flags as decimal digits for easy reading
+
+                // the t1Cnt simply allows the telemetry view to show at least some changes
+                t1Cnt++;
+                if (t1Cnt >= 4) {
+                    t1Cnt = 1;
+                }
+                tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
+                // the Taranis seems to be able to fit 5 digits on the screen
+                // the Taranis seems to consider this number a signed 16 bit integer
+
+                if (ARMING_FLAG(OK_TO_ARM))
+                    tmpi += 1;
+                if (ARMING_FLAG(PREVENT_ARMING))
+                    tmpi += 2;
+                if (ARMING_FLAG(ARMED))
+                    tmpi += 4;
+
+                if (FLIGHT_MODE(ANGLE_MODE))
+                    tmpi += 10;
+                if (FLIGHT_MODE(HORIZON_MODE))
+                    tmpi += 20;
+                if (FLIGHT_MODE(AUTOTUNE_MODE))
+                    tmpi += 40;
+                if (FLIGHT_MODE(PASSTHRU_MODE))
+                    tmpi += 40;
+
+                if (FLIGHT_MODE(MAG_MODE))
+                    tmpi += 100;
+                if (FLIGHT_MODE(BARO_MODE))
+                    tmpi += 200;
+                if (FLIGHT_MODE(SONAR_MODE))
+                    tmpi += 400;
+
+                if (FLIGHT_MODE(GPS_HOLD_MODE))
+                    tmpi += 1000;
+                if (FLIGHT_MODE(GPS_HOME_MODE))
+                    tmpi += 2000;
+                if (FLIGHT_MODE(HEADFREE_MODE))
+                    tmpi += 4000;
+
+                smartPortSendPackage(id, (uint32_t)tmpi);
+                smartPortHasRequest = 0;
+                break;
+            case FSSP_DATAID_T2         :
+                if (sensors(SENSOR_GPS)) {
+                    // provide GPS lock status
+                    smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
+                    smartPortHasRequest = 0;
+                }
+                else {
+                    smartPortSendPackage(id, 0);
+                    smartPortHasRequest = 0;
+                }
+                break;
+            case FSSP_DATAID_GPS_ALT    :
+                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
+                    smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m
+                    smartPortHasRequest = 0;
+                }
+                break;
+            default:
+                break;
+                // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just wait for the next loop
+        }
+    }
+}
+
+#endif

src/main/telemetry/smartport.h

@@ -0,0 +1,24 @@
+/*
+ * smartport.h
+ *
+ *  Created on: 25 October 2014
+ *      Author: Frank26080115
+ */
+
+#ifndef TELEMETRY_SMARTPORT_H_
+#define TELEMETRY_SMARTPORT_H_
+
+void initSmartPortTelemetry(telemetryConfig_t *);
+
+void handleSmartPortTelemetry(void);
+void checkSmartPortTelemetryState(void);
+
+void configureSmartPortTelemetryPort(void);
+void freeSmartPortTelemetryPort(void);
+
+uint32_t getSmartPortTelemetryProviderBaudRate(void);
+
+bool canSmartPortAllowOtherSerial(void);
+bool isSmartPortTimedOut(void);
+
+#endif /* TELEMETRY_SMARTPORT_H_ */

src/main/telemetry/telemetry.c

@@ -39,6 +39,7 @@
 #include "telemetry/frsky.h"
 #include "telemetry/hott.h"
 #include "telemetry/msp.h"
+#include "telemetry/smartport.h"
 
 
 static bool isTelemetryConfigurationValid = false; // flag used to avoid repeated configuration checks
@@ -67,13 +68,27 @@ bool isTelemetryProviderMSP(void)
     return telemetryConfig->telemetry_provider == TELEMETRY_PROVIDER_MSP;
 }
 
+bool isTelemetryProviderSmartPort(void)
+{
+    return telemetryConfig->telemetry_provider == TELEMETRY_PROVIDER_SMARTPORT;
+}
+
+bool isTelemetryPortShared(void)
+{
+    return telemetryPortIsShared;
+}
+
 bool canUseTelemetryWithCurrentConfiguration(void)
 {
     if (!feature(FEATURE_TELEMETRY)) {
         return false;
     }
 
-    if (!canOpenSerialPort(FUNCTION_TELEMETRY)) {
+    if (telemetryConfig->telemetry_provider != TELEMETRY_PROVIDER_SMARTPORT && !canOpenSerialPort(FUNCTION_TELEMETRY)) {
+        return false;
+    }
+
+    if (telemetryConfig->telemetry_provider == TELEMETRY_PROVIDER_SMARTPORT && !canOpenSerialPort(FUNCTION_SMARTPORT_TELEMETRY)) {
         return false;
     }
 
@@ -82,7 +97,7 @@ bool canUseTelemetryWithCurrentConfiguration(void)
 
 void initTelemetry()
 {
-    telemetryPortIsShared = isSerialPortFunctionShared(FUNCTION_TELEMETRY, FUNCTION_MSP);
+    telemetryPortIsShared = isSerialPortFunctionShared(FUNCTION_TELEMETRY, FUNCTION_MSP) | isSerialPortFunctionShared(FUNCTION_SMARTPORT_TELEMETRY, FUNCTION_MSP);
     isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration();
 
     if (isTelemetryProviderFrSky()) {
@@ -97,6 +112,10 @@ void initTelemetry()
         initMSPTelemetry(telemetryConfig);
     }
 
+    if (isTelemetryProviderSmartPort()) {
+        initSmartPortTelemetry(telemetryConfig);
+    }
+
     checkTelemetryState();
 }
 
@@ -105,10 +124,17 @@ bool determineNewTelemetryEnabledState(void)
     bool enabled = true;
 
     if (telemetryPortIsShared) {
-        if (telemetryConfig->telemetry_switch)
+        if (telemetryConfig->telemetry_provider == TELEMETRY_PROVIDER_SMARTPORT) {
-            enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY);
+            if (isSmartPortTimedOut()) {
-        else
+                enabled = false;
-            enabled = ARMING_FLAG(ARMED);
+            }
+        }
+        else {
+            if (telemetryConfig->telemetry_switch)
+                enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY);
+            else
+                enabled = ARMING_FLAG(ARMED);
+        }
     }
 
     return enabled;
@@ -132,6 +158,11 @@ uint32_t getTelemetryProviderBaudRate(void)
     if (isTelemetryProviderMSP()) {
         return getMSPTelemetryProviderBaudRate();
     }
+
+    if (isTelemetryProviderSmartPort()) {
+        return getSmartPortTelemetryProviderBaudRate();
+    }
+
     return 0;
 }
 
@@ -148,6 +179,10 @@ static void configureTelemetryPort(void)
     if (isTelemetryProviderMSP()) {
         configureMSPTelemetryPort();
     }
+
+    if (isTelemetryProviderSmartPort()) {
+        configureSmartPortTelemetryPort();
+    }
 }
 
 
@@ -164,6 +199,10 @@ void freeTelemetryPort(void)
     if (isTelemetryProviderMSP()) {
         freeMSPTelemetryPort();
     }
+
+    if (isTelemetryProviderSmartPort()) {
+        freeSmartPortTelemetryPort();
+    }
 }
 
 void checkTelemetryState(void)
@@ -206,5 +245,23 @@ void handleTelemetry(void)
     if (isTelemetryProviderMSP()) {
         handleMSPTelemetry();
     }
+
+    if (isTelemetryProviderSmartPort()) {
+        handleSmartPortTelemetry();
+    }
 }
+
+bool telemetryAllowsOtherSerial(int serialPortFunction)
+{
+    if (!feature(FEATURE_TELEMETRY)) {
+        return true;
+    }
+
+    if (isTelemetryProviderSmartPort() && isSerialPortFunctionShared(FUNCTION_SMARTPORT_TELEMETRY, (serialPortFunction_e)serialPortFunction)) {
+        return canSmartPortAllowOtherSerial();
+    }
+
+    return true;
+}
+
 #endif

src/main/telemetry/telemetry.h

@@ -29,7 +29,8 @@ typedef enum {
     TELEMETRY_PROVIDER_FRSKY = 0,
     TELEMETRY_PROVIDER_HOTT,
     TELEMETRY_PROVIDER_MSP,
-    TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_MSP
+    TELEMETRY_PROVIDER_SMARTPORT,
+    TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_SMARTPORT
 } telemetryProvider_e;
 
 typedef enum {
@@ -44,7 +45,7 @@ typedef enum {
 typedef struct telemetryConfig_s {
     telemetryProvider_e telemetry_provider;
     uint8_t telemetry_switch;               // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
-    serialInversion_e frsky_inversion;
+    serialInversion_e telemetry_inversion;      // also shared with smartport inversion
 	float gpsNoFixLatitude;   
     float gpsNoFixLongitude;  
     frskyGpsCoordFormat_e frsky_coordinate_format;   
@@ -57,5 +58,7 @@ void handleTelemetry(void);
 
 uint32_t getTelemetryProviderBaudRate(void);
 void useTelemetryConfig(telemetryConfig_t *telemetryConfig);
+bool telemetryAllowsOtherSerial(int serialPortFunction);
+bool isTelemetryPortShared(void);
 
 #endif /* TELEMETRY_COMMON_H_ */