rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updated handling of FrSky telemetry processing, renamed to `FrSkyHub` for disambiguation.

This commit is contained in:
mikeller 2017-12-26 15:15:15 +13:00 committed by Michael Keller
parent 2207a98a7a
commit 0b9884961d
19 changed files with 244 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
make/source.mk
View File

@ -175,7 +175,7 @@ FC_SRC = \
telemetry/telemetry.c \
telemetry/crsf.c \
telemetry/srxl.c \
telemetry/frsky.c \
telemetry/frsky_hub.c \
telemetry/hott.c \
telemetry/jetiexbus.c \
telemetry/smartport.c \

2
src/main/interface/cli.c
View File

@ -136,7 +136,7 @@ extern uint8_t __config_end;
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "telemetry/frsky.h"
#include "telemetry/frsky_hub.h"
#include "telemetry/telemetry.h"

6
src/main/interface/settings.c
View File

@ -84,7 +84,7 @@
#include "sensors/gyro.h"
#include "sensors/rangefinder.h"
#include "telemetry/frsky.h"
#include "telemetry/frsky_hub.h"
#include "telemetry/telemetry.h"
// Sensor names (used in lookup tables for *_hardware settings and in status command output)
@ -679,7 +679,7 @@ const clivalue_t valueTable[] = {
{ "tlm_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_switch) },
{ "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },
{ "tlm_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, halfDuplex) },
#if defined(USE_TELEMETRY_FRSKY)
#if defined(USE_TELEMETRY_FRSKY_HUB)
#if defined(USE_GPS)
{ "frsky_default_lat", VAR_INT16 | MASTER_VALUE, .config.minmax = { -9000, 9000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) },
{ "frsky_default_long", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLongitude) },
@ -687,7 +687,7 @@ const clivalue_t valueTable[] = {
{ "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_UNIT }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_unit) },
#endif
{ "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_vfas_precision) },
#endif // USE_TELEMETRY_FRSKY
#endif // USE_TELEMETRY_FRSKY_HUB
{ "hott_alarm_int", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 120 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, hottAlarmSoundInterval) },
{ "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pidValuesAsTelemetry) },
#if defined(USE_TELEMETRY_IBUS)

2
src/main/io/serial.h
View File

@ -33,7 +33,7 @@ typedef enum {
FUNCTION_NONE = 0,
FUNCTION_MSP = (1 << 0), // 1
FUNCTION_GPS = (1 << 1), // 2
FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4
FUNCTION_TELEMETRY_FRSKY_HUB = (1 << 2), // 4
FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8
FUNCTION_TELEMETRY_LTM = (1 << 4), // 16
FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32

69
src/main/rx/cc2500_frsky_d.c
View File

@ -43,7 +43,7 @@
#include "sensors/battery.h"
#include "telemetry/frsky.h"
#include "telemetry/frsky_hub.h"
#include "cc2500_frsky_d.h"
@ -51,51 +51,51 @@
static uint8_t frame[20];
static uint8_t telemetryId;
#if defined(USE_TELEMETRY_FRSKY)
#if defined(USE_TELEMETRY_FRSKY_HUB)
static bool telemetryEnabled = false;
#define MAX_SERIAL_BYTES 64
static uint8_t hubIndex;
static uint8_t telemetryInxdex = 0;
static uint8_t telemetryBytesGenerated;
static uint8_t serialBuffer[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
#endif
#endif // USE_RX_FRSKY_SPI_TELEMETRY
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
#if defined(USE_TELEMETRY_FRSKY)
static uint8_t appendFrSkyHubData(uint8_t *buf)
{
static uint8_t telemetryIndexAcknowledged = 0;
static uint8_t telemetryIndexExpected = 0;
static uint8_t telemetryBytesSent = 0;
static uint8_t telemetryBytesAcknowledged = 0;
static uint8_t telemetryIdExpected = 0;
if (telemetryId == telemetryIndexExpected) {
telemetryIndexAcknowledged = telemetryInxdex;
if (telemetryId == telemetryIdExpected) {
telemetryBytesAcknowledged = telemetryBytesSent;
telemetryIdExpected = (telemetryId + 1) & 0x1F;
if (!telemetryBytesGenerated) {
telemetryBytesSent = 0;
processFrSkyHubTelemetry(micros());
}
} else { // rx re-requests last packet
telemetryInxdex = telemetryIndexAcknowledged;
telemetryBytesSent = telemetryBytesAcknowledged;
}
telemetryIndexExpected = (telemetryId + 1) & 0x1F;
uint8_t index = 0;
for (uint8_t i = 0; i < 10; i++) {
if (telemetryInxdex == hubIndex) {
if (telemetryBytesSent == telemetryBytesGenerated) {
telemetryBytesGenerated = 0;
break;
}
buf[i] = serialBuffer[telemetryInxdex];
telemetryInxdex = (telemetryInxdex + 1) & (MAX_SERIAL_BYTES - 1);
buf[i] = serialBuffer[telemetryBytesSent];
telemetryBytesSent = (telemetryBytesSent + 1) & (MAX_SERIAL_BYTES - 1);
index++;
}
return index;
}
static void frSkyTelemetryInitFrameSpi(void)
static void frSkyDTelemetryWriteByte(const char data)
{
hubIndex = 0;
telemetryInxdex = 0;
}
static void frSkyTelemetryWriteSpi(uint8_t ch)
{
if (hubIndex < MAX_SERIAL_BYTES) {
serialBuffer[hubIndex++] = ch;
if (telemetryBytesGenerated < MAX_SERIAL_BYTES) {
serialBuffer[telemetryBytesGenerated++] = data;
}
}
#endif
@ -104,7 +104,6 @@ static void buildTelemetryFrame(uint8_t *packet)
{
const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
uint8_t bytes_used = 0;
telemetryId = packet[4];
frame[0] = 0x11; // length
frame[1] = rxFrSkySpiConfig()->bindTxId[0];
@ -112,10 +111,13 @@ static void buildTelemetryFrame(uint8_t *packet)
frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2
frame[5] = (uint8_t)rssiDbm;
#if defined(USE_TELEMETRY_FRSKY)
bytes_used = appendFrSkyHubData(&frame[8]);
#endif
frame[6] = bytes_used;
uint8_t bytesUsed = 0;
#if defined(USE_TELEMETRY_FRSKY_HUB)
if (telemetryEnabled) {
bytesUsed = appendFrSkyHubData(&frame[8]);
}
#endif
frame[6] = bytesUsed;
frame[7] = telemetryId;
}
#endif // USE_RX_FRSKY_SPI_TELEMETRY
@ -289,9 +291,8 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
void frSkyDInit(void)
{
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY)
initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
&frSkyTelemetryWriteSpi);
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY_HUB)
telemetryEnabled = initFrSkyHubTelemetryExternal(frSkyDTelemetryWriteByte);
#endif
}
#endif

2
src/main/target/ALIENFLIGHTF3/config.c
View File

@ -96,7 +96,7 @@ void targetConfiguration(void)
} else {
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->serialrx_inverted = true;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL;
telemetryConfigMutable()->telemetry_inverted = false;
featureSet(FEATURE_TELEMETRY);
beeperDevConfigMutable()->isOpenDrain = false;

2
src/main/target/ALIENFLIGHTF4/config.c
View File

@ -64,7 +64,7 @@ void targetConfiguration(void)
} else {
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->serialrx_inverted = true;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL;
telemetryConfigMutable()->telemetry_inverted = false;
batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC;
batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC;

2
src/main/target/ALIENFLIGHTNGF7/config.c
View File

@ -63,7 +63,7 @@ void targetConfiguration(void)
} else {
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->serialrx_inverted = true;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL;
telemetryConfigMutable()->telemetry_inverted = false;
batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC;
batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC;

2
src/main/target/BEEBRAIN_V2F/config.c
View File

@ -150,7 +150,7 @@ void targetConfiguration(void)
}
#else
// Frsky version
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL;
rxConfigMutable()->rssi_channel = BBV2_FRSKY_RSSI_CH_IDX;
rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigsMutable(BBV2_FRSKY_RSSI_CH_IDX - 1);
channelFailsafeConfig->mode = RX_FAILSAFE_MODE_SET;

2
src/main/target/FF_PIKOBLX/config.c
View File

@ -58,7 +58,7 @@ void targetConfiguration(void)
rxConfigMutable()->spektrum_sat_bind = 5;
rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
#else
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_TELEMETRY_FRSKY;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB;
rxConfigMutable()->serialrx_inverted = true;
featureSet(FEATURE_TELEMETRY);
#endif

2
src/main/target/SITL/target.h
View File

@ -98,7 +98,7 @@
#undef USE_SERIALRX_SUMH
#undef USE_SERIALRX_XBUS
#undef USE_LED_STRIP
#undef USE_TELEMETRY_FRSKY
#undef USE_TELEMETRY_FRSKY_HUB
#undef USE_TELEMETRY_HOTT
#undef USE_TELEMETRY_SMARTPORT
#undef USE_TELEMETRY_MAVLINK

2
src/main/target/TINYFISH/config.c
View File

@ -41,7 +41,7 @@ void targetConfiguration(void)
{
// use the same uart for frsky telemetry and SBUS, both non inverted
const int index = findSerialPortIndexByIdentifier(SBUS_TELEMETRY_UART);
serialConfigMutable()->portConfigs[index].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
serialConfigMutable()->portConfigs[index].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL;
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->serialrx_inverted = true;

2
src/main/target/common_fc_pre.h
View File

@ -119,7 +119,7 @@
#define USE_BLACKBOX
#define USE_LED_STRIP
#define USE_TELEMETRY
#define USE_TELEMETRY_FRSKY
#define USE_TELEMETRY_FRSKY_HUB
#define USE_TELEMETRY_HOTT
#define USE_TELEMETRY_LTM
#define USE_TELEMETRY_SMARTPORT

329
src/main/telemetry/frsky.c → src/main/telemetry/frsky_hub.c
View File

@ -16,7 +16,7 @@
*/
/*
* Initial FrSky Telemetry implementation by silpstream @ rcgroups.
* Initial FrSky Hub Telemetry implementation by silpstream @ rcgroups.
* Addition protocol work by airmamaf @ github.
*/
@ -26,7 +26,7 @@
#include "platform.h"
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_FRSKY)
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_FRSKY_HUB)
#include "common/maths.h"
#include "common/axis.h"
@ -62,24 +62,24 @@
#include "rx/rx.h"
#include "telemetry/telemetry.h"
#include "telemetry/frsky.h"
#if defined(USE_ESC_SENSOR)
#include "sensors/esc_sensor.h"
#endif
static serialPort_t *frskyPort = NULL;
#include "frsky_hub.h"
static serialPort_t *frSkyHubPort = NULL;
static serialPortConfig_t *portConfig = NULL;
#define FRSKY_BAUDRATE 9600
#define FRSKY_INITIAL_PORT_MODE MODE_TX
#define FRSKY_HUB_BAUDRATE 9600
#define FRSKY_HUB_INITIAL_PORT_MODE MODE_TX
static portSharing_e frskyPortSharing;
static portSharing_e frSkyHubPortSharing;
static frSkyTelemetryWriteFn *frSkyTelemetryWrite = NULL;
static frSkyTelemetryInitFrameFn *frSkyTelemetryInitFrame = NULL;
static frSkyHubWriteByteFn *frSkyHubWriteByte = NULL;
#define FRSKY_CYCLETIME_US 125000
#define FRSKY_HUB_CYCLETIME_US 125000
#define PROTOCOL_HEADER 0x5E
#define PROTOCOL_TAIL 0x5E
@ -128,80 +128,89 @@ static frSkyTelemetryInitFrameFn *frSkyTelemetryInitFrame = NULL;
#define DELAY_FOR_BARO_INITIALISATION_US 5000000
#define BLADE_NUMBER_DIVIDER 5 // should set 12 blades in Taranis
static uint32_t frskyLastCycleTime = 0;
static uint8_t cycleNum = 0;
static void sendDataHead(uint8_t id)
enum
{
frSkyTelemetryWrite(PROTOCOL_HEADER);
frSkyTelemetryWrite(id);
TELEMETRY_STATE_UNINITIALIZED,
TELEMETRY_STATE_INITIALIZED_SERIAL,
TELEMETRY_STATE_INITIALIZED_EXTERNAL,
};
static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
static void serializeFrSkyHub(uint8_t data)
{
// take care of byte stuffing
if (data == 0x5e) {
frSkyHubWriteByte(0x5d);
frSkyHubWriteByte(0x3e);
} else if (data == 0x5d) {
frSkyHubWriteByte(0x5d);
frSkyHubWriteByte(0x3d);
} else{
frSkyHubWriteByte(data);
}
}
static void frSkyHubWriteFrame(const uint8_t id, const int16_t data)
{
frSkyHubWriteByte(PROTOCOL_HEADER);
frSkyHubWriteByte(id);
serializeFrSkyHub((uint8_t)data);
serializeFrSkyHub(data >> 8);
}
static void sendTelemetryTail(void)
{
frSkyTelemetryWrite(PROTOCOL_TAIL);
frSkyHubWriteByte(PROTOCOL_TAIL);
}
static void serializeFrsky(uint8_t data)
{
// take care of byte stuffing
if (data == 0x5e) {
frSkyTelemetryWrite(0x5d);
frSkyTelemetryWrite(0x3e);
} else if (data == 0x5d) {
frSkyTelemetryWrite(0x5d);
frSkyTelemetryWrite(0x3d);
} else{
frSkyTelemetryWrite(data);
}
}
static void serialize16(int16_t data)
{
serializeFrsky((uint8_t)data);
serializeFrsky(data >> 8);
}
static void frSkyHubWriteByteInternal(const char data)
{
serialWrite(frSkyHubPort, data);
}
static void sendAccel(void)
{
int i;
for (i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i);
serialize16(((float)acc.accSmooth[i] / acc.dev.acc_1G) * 1000);
for (unsigned i = 0; i < 3; i++) {
frSkyHubWriteFrame(ID_ACC_X + i, ((float)acc.accSmooth[i] / acc.dev.acc_1G) * 1000);
}
}
static void sendThrottleOrBatterySizeAsRpm(void)
{
sendDataHead(ID_RPM);
int16_t data;
#if defined(USE_ESC_SENSOR)
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0);
data = escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0;
#else
if (ARMING_FLAG(ARMED)) {
const throttleStatus_e throttleStatus = calculateThrottleStatus();
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
throttleForRPM = 0;
serialize16(throttleForRPM);
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
throttleForRPM = 0;
}
data = throttleForRPM;
} else {
serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER));
data = (batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER);
}
#endif
frSkyHubWriteFrame(ID_RPM, data);
}
static void sendTemperature1(void)
{
sendDataHead(ID_TEMPRATURE1);
int16_t data;
#if defined(USE_ESC_SENSOR)
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0);
data = escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0;
#elif defined(USE_BARO)
serialize16((baro.baroTemperature + 50)/ 100); // Airmamaf
data = (baro.baroTemperature + 50)/ 100; // Airmamaf
#else
serialize16(gyroGetTemperature() / 10);
data = gyroGetTemperature() / 10;
#endif
frSkyHubWriteFrame(ID_TEMPRATURE1, data);
}
static void sendTime(void)
@ -210,10 +219,8 @@ static void sendTime(void)
uint8_t minutes = (seconds / 60) % 60;
// if we fly for more than an hour, something's wrong anyway
sendDataHead(ID_HOUR_MINUTE);
serialize16(minutes << 8);
sendDataHead(ID_SECOND);
serialize16(seconds % 60);
frSkyHubWriteFrame(ID_HOUR_MINUTE, minutes << 8);
frSkyHubWriteFrame(ID_SECOND, seconds % 60);
}
#if defined(USE_GPS) || defined(USE_MAG)
@ -241,20 +248,14 @@ static void sendLatLong(int32_t coord[2])
{
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LATITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_N_S);
serialize16(coord[LAT] < 0 ? 'S' : 'N');
frSkyHubWriteFrame(ID_LATITUDE_BP, coordinate.dddmm);
frSkyHubWriteFrame(ID_LATITUDE_AP, coordinate.mmmm);
frSkyHubWriteFrame(ID_N_S, coord[LAT] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(coord[LON], &coordinate);
sendDataHead(ID_LONGITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LONGITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_E_W);
serialize16(coord[LON] < 0 ? 'W' : 'E');
frSkyHubWriteFrame(ID_LONGITUDE_BP, coordinate.dddmm);
frSkyHubWriteFrame(ID_LONGITUDE_AP, coordinate.mmmm);
frSkyHubWriteFrame(ID_E_W, coord[LON] < 0 ? 'W' : 'E');
}
#if defined(USE_GPS)
@ -266,28 +267,27 @@ static void sendGpsAltitude(void)
if (!STATE(GPS_FIX)) {
altitude = 0;
}
sendDataHead(ID_GPS_ALTIDUTE_BP);
serialize16(altitude);
sendDataHead(ID_GPS_ALTIDUTE_AP);
serialize16(0);
frSkyHubWriteFrame(ID_GPS_ALTIDUTE_BP, altitude);
frSkyHubWriteFrame(ID_GPS_ALTIDUTE_AP, 0);
}
static void sendSatalliteSignalQualityAsTemperature2(void)
static void sendSatalliteSignalQualityAsTemperature2(uint8_t cycleNum)
{
uint16_t satellite = gpsSol.numSat;
if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) { // Every 1s
satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
}
sendDataHead(ID_TEMPRATURE2);
int16_t data;
if (telemetryConfig()->frsky_unit == FRSKY_UNIT_METRICS) {
serialize16(satellite);
data = satellite;
} else {
float tmp = (satellite - 32) / 1.8f;
// Round the value
tmp += (tmp < 0) ? -0.5f : 0.5f;
serialize16(tmp);
data = tmp;
}
frSkyHubWriteFrame(ID_TEMPRATURE2, data);
}
static void sendSpeed(void)
@ -296,11 +296,9 @@ static void sendSpeed(void)
return;
}
// Speed should be sent in knots (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP);
// convert to knots: 1cm/s = 0.0194384449 knots
serialize16(gpsSol.groundSpeed * 1944 / 100000);
sendDataHead(ID_GPS_SPEED_AP);
serialize16((gpsSol.groundSpeed * 1944 / 100) % 100);
frSkyHubWriteFrame(ID_GPS_SPEED_BP, gpsSol.groundSpeed * 1944 / 100000);
frSkyHubWriteFrame(ID_GPS_SPEED_AP, (gpsSol.groundSpeed * 1944 / 100) % 100);
}
static void sendFakeLatLong(void)
@ -341,8 +339,7 @@ static void sendGPSLatLong(void)
*/
static void sendVario(void)
{
sendDataHead(ID_VERT_SPEED);
serialize16(getEstimatedVario());
frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario());
}
#endif
@ -373,16 +370,15 @@ static void sendVoltageCells(void)
uint32_t cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2);
// Cell number is at bit 9-12
uint16_t payload = (currentCell << 4);
uint16_t data = (currentCell << 4);
// Lower voltage bits are at bit 0-8
payload |= ((cellVoltage & 0x0ff) << 8);
data |= ((cellVoltage & 0x0ff) << 8);
// Higher voltage bits are at bits 13-15
payload |= ((cellVoltage & 0xf00) >> 8);
data |= ((cellVoltage & 0xf00) >> 8);
sendDataHead(ID_VOLT);
serialize16(payload);
frSkyHubWriteFrame(ID_VOLT, data);
currentCell++;
}
@ -396,8 +392,7 @@ static void sendVoltageAmp(void)
if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
// Use new ID 0x39 to send voltage directly in 0.1 volts resolution
sendDataHead(ID_VOLTAGE_AMP);
serialize16(voltage);
frSkyHubWriteFrame(ID_VOLTAGE_AMP, voltage);
} else {
// send in 0.2 volts resolution
voltage *= 110 / 21;
@ -405,28 +400,25 @@ static void sendVoltageAmp(void)
voltage /= getBatteryCellCount();
}
sendDataHead(ID_VOLTAGE_AMP_BP);
serialize16(voltage / 100);
sendDataHead(ID_VOLTAGE_AMP_AP);
serialize16(((voltage % 100) + 5) / 10);
frSkyHubWriteFrame(ID_VOLTAGE_AMP_BP, voltage / 100);
frSkyHubWriteFrame(ID_VOLTAGE_AMP_AP, ((voltage % 100) + 5) / 10);
}
}
static void sendAmperage(void)
{
sendDataHead(ID_CURRENT);
serialize16((uint16_t)(getAmperage() / 10));
frSkyHubWriteFrame(ID_CURRENT, (uint16_t)(getAmperage() / 10));
}
static void sendFuelLevel(void)
{
sendDataHead(ID_FUEL_LEVEL);
int16_t data;
if (batteryConfig()->batteryCapacity > 0) {
serialize16((uint16_t)calculateBatteryPercentageRemaining());
data = (uint16_t)calculateBatteryPercentageRemaining();
} else {
serialize16((uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF));
data = (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF);
}
frSkyHubWriteFrame(ID_FUEL_LEVEL, data);
}
#if defined(USE_MAG)
@ -443,103 +435,85 @@ static void sendFakeLatLongThatAllowsHeadingDisplay(void)
static void sendHeading(void)
{
sendDataHead(ID_COURSE_BP);
serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
sendDataHead(ID_COURSE_AP);
serialize16(0);
frSkyHubWriteFrame(ID_COURSE_BP, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
frSkyHubWriteFrame(ID_COURSE_AP, 0);
}
#endif
static void frSkyTelemetryWriteSerial(uint8_t ch)
bool initFrSkyHubTelemetry(void)
{
serialWrite(frskyPort, ch);
}
if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY_HUB);
if (portConfig) {
frSkyHubPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY_HUB);
frSkyHubWriteByte = frSkyHubWriteByteInternal;
void initFrSkyTelemetry(void)
{
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY);
frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY);
}
void initFrSkyExternalTelemetry(frSkyTelemetryInitFrameFn *frSkyTelemetryInitFrameExternal, frSkyTelemetryWriteFn *frSkyTelemetryWriteExternal)
{
frSkyTelemetryInitFrame = frSkyTelemetryInitFrameExternal;
frSkyTelemetryWrite = frSkyTelemetryWriteExternal;
}
void deinitFrSkyExternalTelemetry(void)
{
frSkyTelemetryInitFrame = NULL;
frSkyTelemetryWrite = NULL;
}
void freeFrSkyTelemetryPort(void)
{
if (frskyPort) {
closeSerialPort(frskyPort);
frskyPort = NULL;
frSkyTelemetryWrite = NULL;
}
}
static void configureFrSkyTelemetryPort(void)
{
if (portConfig) {
frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
if (!frskyPort) {
return;
telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
}
frSkyTelemetryWrite = frSkyTelemetryWriteSerial;
return true;
}
return false;
}
bool initFrSkyHubTelemetryExternal(frSkyHubWriteByteFn *frSkyHubWriteByteExternal)
{
if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
frSkyHubWriteByte = frSkyHubWriteByteExternal;
telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
return true;
}
return false;
}
void freeFrSkyHubTelemetryPort(void)
{
closeSerialPort(frSkyHubPort);
frSkyHubPort = NULL;
}
static void configureFrSkyHubTelemetryPort(void)
{
if (portConfig) {
frSkyHubPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY_HUB, NULL, NULL, FRSKY_HUB_BAUDRATE, FRSKY_HUB_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
}
}
bool checkFrSkySerialTelemetryEnabled(void)
void checkFrSkyHubTelemetryState(void)
{
return frSkyTelemetryWrite == &frSkyTelemetryWriteSerial;
}
void checkFrSkyTelemetryState(void)
{
if (portConfig) {
if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
if (telemetryCheckRxPortShared(portConfig)) {
if (!checkFrSkySerialTelemetryEnabled() && telemetrySharedPort != NULL) {
frskyPort = telemetrySharedPort;
frSkyTelemetryWrite = &frSkyTelemetryWriteSerial;
if (frSkyHubPort == NULL && telemetrySharedPort != NULL) {
frSkyHubPort = telemetrySharedPort;
}
} else {
bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing);
if (newTelemetryEnabledValue == checkFrSkySerialTelemetryEnabled()) {
return;
}
if (newTelemetryEnabledValue) {
configureFrSkyTelemetryPort();
} else {
freeFrSkyTelemetryPort();
bool enableSerialTelemetry = telemetryDetermineEnabledState(frSkyHubPortSharing);
if (enableSerialTelemetry && !frSkyHubPort) {
configureFrSkyHubTelemetryPort();
} else if (!enableSerialTelemetry && frSkyHubPort) {
freeFrSkyHubTelemetryPort();
}
}
}
}
void handleFrSkyTelemetry(timeUs_t currentTimeUs)
void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
{
if (frSkyTelemetryWrite == NULL) {
return;
}
static uint32_t frSkyHubLastCycleTime = 0;
static uint8_t cycleNum = 0;
if (currentTimeUs < frskyLastCycleTime + FRSKY_CYCLETIME_US) {
if (cmpTimeUs(currentTimeUs, frSkyHubLastCycleTime) < FRSKY_HUB_CYCLETIME_US) {
return;
}
frskyLastCycleTime = currentTimeUs;
frSkyHubLastCycleTime = currentTimeUs;
cycleNum++;
if (frSkyTelemetryInitFrame) {
frSkyTelemetryInitFrame();
}
if (sensors(SENSOR_ACC)) {
// Sent every 125ms
sendAccel();
@ -556,14 +530,12 @@ void handleFrSkyTelemetry(timeUs_t currentTimeUs)
/* Allow 5s to boot correctly othervise send zero to prevent OpenTX
* sensor lost notifications after warm boot. */
if (frskyLastCycleTime < DELAY_FOR_BARO_INITIALISATION_US) {
if (frSkyHubLastCycleTime < DELAY_FOR_BARO_INITIALISATION_US) {
altitude = 0;
}
sendDataHead(ID_ALTITUDE_BP);
serialize16(altitude / 100);
sendDataHead(ID_ALTITUDE_AP);
serialize16(altitude % 100);
frSkyHubWriteFrame(ID_ALTITUDE_BP, altitude / 100);
frSkyHubWriteFrame(ID_ALTITUDE_AP, altitude % 100);
}
}
#endif
@ -596,7 +568,7 @@ void handleFrSkyTelemetry(timeUs_t currentTimeUs)
if (sensors(SENSOR_GPS)) {
sendSpeed();
sendGpsAltitude();
sendSatalliteSignalQualityAsTemperature2();
sendSatalliteSignalQualityAsTemperature2(cycleNum);
sendGPSLatLong();
} else
#endif
@ -617,4 +589,11 @@ void handleFrSkyTelemetry(timeUs_t currentTimeUs)
sendTelemetryTail();
}
void handleFrSkyHubTelemetry(timeUs_t currentTimeUs)
{
if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && frSkyHubPort) {
processFrSkyHubTelemetry(currentTimeUs);
}
}
#endif

14
src/main/telemetry/frsky.h → src/main/telemetry/frsky_hub.h
View File

@ -24,12 +24,12 @@ typedef enum {
FRSKY_VFAS_PRECISION_HIGH
} frskyVFasPrecision_e;
typedef void frSkyTelemetryInitFrameFn(void);
typedef void frSkyTelemetryWriteFn(uint8_t ch);
typedef void frSkyHubWriteByteFn(const char data);
void handleFrSkyTelemetry(timeUs_t currentTimeUs);
void checkFrSkyTelemetryState(void);
void initFrSkyTelemetry(void);
void handleFrSkyHubTelemetry(timeUs_t currentTimeUs);
void checkFrSkyHubTelemetryState(void);
void initFrSkyExternalTelemetry(frSkyTelemetryInitFrameFn *frSkyTelemetryInitFrameExternal, frSkyTelemetryWriteFn *frSkyTelemetryWriteExternal);
void deinitFrSkyExternalTelemetry(void);
bool initFrSkyHubTelemetry(void);
bool initFrSkyHubTelemetryExternal(frSkyHubWriteByteFn *frSkyWriteFrameExternal);
void processFrSkyHubTelemetry(timeUs_t currentTimeUs);

55
src/main/telemetry/smartport.c
View File

@ -58,14 +58,7 @@
#include "telemetry/smartport.h"
#include "telemetry/msp_shared.h"
enum
{
SPSTATE_UNINITIALIZED,
SPSTATE_INITIALIZED_SERIAL,
SPSTATE_INITIALIZED_EXTERNAL,
};
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
enum
{
FSSP_DATAID_SPEED = 0x0830 ,
@ -132,7 +125,14 @@ static serialPortConfig_t *portConfig;
static portSharing_e smartPortPortSharing;
char smartPortState = SPSTATE_UNINITIALIZED;
enum
{
TELEMETRY_STATE_UNINITIALIZED,
TELEMETRY_STATE_INITIALIZED_SERIAL,
TELEMETRY_STATE_INITIALIZED_EXTERNAL,
};
static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
static uint8_t smartPortIdCnt = 0;
typedef struct smartPortFrame_s {
@ -255,14 +255,14 @@ static void smartPortSendPackage(uint16_t id, uint32_t val)
bool initSmartPortTelemetry(void)
{
if (smartPortState == SPSTATE_UNINITIALIZED) {
if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
if (portConfig) {
smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
smartPortWriteFrame = smartPortWriteFrameInternal;
smartPortState = SPSTATE_INITIALIZED_SERIAL;
telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
}
return true;
@ -273,10 +273,10 @@ bool initSmartPortTelemetry(void)
bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
{
if (smartPortState == SPSTATE_UNINITIALIZED) {
if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
smartPortWriteFrame = smartPortWriteFrameExternal;
smartPortState = SPSTATE_INITIALIZED_EXTERNAL;
telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
return true;
}
@ -292,18 +292,16 @@ static void freeSmartPortTelemetryPort(void)
static void configureSmartPortTelemetryPort(void)
{
if (!portConfig) {
return;
if (portConfig) {
portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
}
portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
}
void checkSmartPortTelemetryState(void)
{
if (smartPortState == SPSTATE_INITIALIZED_SERIAL) {
if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
if (enableSerialTelemetry && !smartPortSerialPort) {
@ -589,17 +587,14 @@ void handleSmartPortTelemetry(void)
const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
if (!(smartPortState == SPSTATE_INITIALIZED_SERIAL && smartPortSerialPort)) {
return;
}
if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
smartPortPayload_t *payload = NULL;
while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
uint8_t c = serialRead(smartPortSerialPort);
payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
}
smartPortPayload_t *payload = NULL;
while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
uint8_t c = serialRead(smartPortSerialPort);
payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
}
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
}
#endif

14
src/main/telemetry/telemetry.c
View File

@ -43,7 +43,7 @@
#include "rx/rx.h"
#include "telemetry/telemetry.h"
#include "telemetry/frsky.h"
#include "telemetry/frsky_hub.h"
#include "telemetry/hott.h"
#include "telemetry/smartport.h"
#include "telemetry/ltm.h"
@ -73,8 +73,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
void telemetryInit(void)
{
#ifdef USE_TELEMETRY_FRSKY
initFrSkyTelemetry();
#ifdef USE_TELEMETRY_FRSKY_HUB
initFrSkyHubTelemetry();
#endif
#ifdef USE_TELEMETRY_HOTT
initHoTTTelemetry();
@ -142,8 +142,8 @@ serialPort_t *telemetrySharedPort = NULL;
void telemetryCheckState(void)
{
#ifdef USE_TELEMETRY_FRSKY
checkFrSkyTelemetryState();
#ifdef USE_TELEMETRY_FRSKY_HUB
checkFrSkyHubTelemetryState();
#endif
#ifdef USE_TELEMETRY_HOTT
checkHoTTTelemetryState();
@ -173,8 +173,8 @@ void telemetryCheckState(void)
void telemetryProcess(uint32_t currentTime)
{
#ifdef USE_TELEMETRY_FRSKY
handleFrSkyTelemetry(currentTime);
#ifdef USE_TELEMETRY_FRSKY_HUB
handleFrSkyHubTelemetry(currentTime);
#else
UNUSED(currentTime);
#endif

2
src/main/telemetry/telemetry.h
View File

@ -54,7 +54,7 @@ typedef struct telemetryConfig_s {
PG_DECLARE(telemetryConfig_t, telemetryConfig);
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
#define TELEMETRY_PORT_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT)
extern serialPort_t *telemetrySharedPort;

2
src/test/unit/target.h
View File

@ -41,7 +41,7 @@
#define USE_SERIALRX_XBUS // JR
#define USE_TELEMETRY
#define USE_TELEMETRY_CRSF
#define USE_TELEMETRY_FRSKY
#define USE_TELEMETRY_FRSKY_HUB
#define USE_TELEMETRY_HOTT
#define USE_TELEMETRY_IBUS
#define USE_TELEMETRY_JETIEXBUS