diff --git a/Makefile b/Makefile index f804d2bb2d..2b39ff4802 100644 --- a/Makefile +++ b/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 diff --git a/src/main/config/config.c b/src/main/config/config.c index b819cc12b0..2a09be4200 100755 --- a/src/main/config/config.c +++ b/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; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 3a185da568..758d7cd48c 100644 --- a/src/main/io/serial.c +++ b/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_GPS, 9600, 115200, AUTOBAUD, SPF_NONE }, - { FUNCTION_GPS_PASSTHROUGH, 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_TELEMETRY, 9600, 19200, NO_AUTOBAUD, SPF_NONE } + { FUNCTION_CLI, 9600, 115200, NO_AUTOBAUD, SPF_NONE }, + { FUNCTION_GPS, 9600, 115200, AUTOBAUD, SPF_NONE }, + { FUNCTION_GPS_PASSTHROUGH, 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_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); - cliInit(serialConfig); +#ifdef TELEMETRY + 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) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index f104b8c155..12bbd12216 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -18,13 +18,14 @@ #pragma once typedef enum { - FUNCTION_NONE = 0, - FUNCTION_MSP = (1 << 0), - FUNCTION_CLI = (1 << 1), - FUNCTION_TELEMETRY = (1 << 2), - FUNCTION_SERIAL_RX = (1 << 3), - FUNCTION_GPS = (1 << 4), - FUNCTION_GPS_PASSTHROUGH = (1 << 5) + FUNCTION_NONE = 0, + FUNCTION_MSP = (1 << 0), + FUNCTION_CLI = (1 << 1), + FUNCTION_TELEMETRY = (1 << 2), + FUNCTION_SMARTPORT_TELEMETRY = (1 << 3), + FUNCTION_SERIAL_RX = (1 << 4), + 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 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 44b8c6797a..0d4d6823f6 100644 --- a/src/main/io/serial_cli.c +++ b/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 }, diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 04ffdbafb5..4b7a6c1e0b 100644 --- a/src/main/telemetry/frsky.c +++ b/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; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c new file mode 100644 index 0000000000..8712e58f43 --- /dev/null +++ b/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 +#include +#include +#include + +#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 diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h new file mode 100644 index 0000000000..5143f96436 --- /dev/null +++ b/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_ */ diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 9a129891f6..6d27b17acf 100644 --- a/src/main/telemetry/telemetry.c +++ b/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) - enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY); - else - enabled = ARMING_FLAG(ARMED); + if (telemetryConfig->telemetry_provider == TELEMETRY_PROVIDER_SMARTPORT) { + if (isSmartPortTimedOut()) { + enabled = false; + } + } + 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 diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 5ed2004b56..a0673651db 100644 --- a/src/main/telemetry/telemetry.h +++ b/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_ */