Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

added smartport telemetry

Browse source 
Conflicts:

	src/main/telemetry/telemetry.h
This commit is contained in:
Frank Zhao 2014-11-04 11:01:08 -08:00 • committed by Dominic Clifton
parent d9c2963182
commit 0000d3e65e
10 changed files with 606 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -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

2
src/main/config/config.c
View file

@ -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;

40
src/main/io/serial.c
View file

@ -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)

18
src/main/io/serial.h
View file

@ -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

2
src/main/io/serial_cli.c
View file

@ -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 },

2
src/main/telemetry/frsky.c
View file

@ -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;

470
src/main/telemetry/smartport.c Normal file
View file

@ -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

24
src/main/telemetry/smartport.h Normal file
View file

@ -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_ */

69
src/main/telemetry/telemetry.c
View file

@ -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

7
src/main/telemetry/telemetry.h
View file

@ -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_ */