Updated handling of FrSky telemetry processing, renamed to `FrSkyHub` for disambiguation.
This commit is contained in:
mikeller
Michael Keller
parent
2207a98a7a
commit
0b9884961d
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 frSkyHubWriteByteInternal(const char data)
|
||||
{
|
||||
serialWrite(frSkyHubPort, data);
|
||||
}
|
||||
}
|
||||
|
||||
static void serialize16(int16_t data)
|
||||
{
|
||||
serializeFrsky((uint8_t)data);
|
||||
serializeFrsky(data >> 8);
|
||||
}
|
||||
|
||||
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))
|
||||
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
|
||||
throttleForRPM = 0;
|
||||
serialize16(throttleForRPM);
|
||||
}
|
||||
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;
|
||||
telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static void configureFrSkyTelemetryPort(void)
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
|
||||
frSkyTelemetryWrite = frSkyTelemetryWriteSerial;
|
||||
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
|
||||
|
|
@ -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);
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
void checkSmartPortTelemetryState(void)
|
||||
{
|
||||
if (smartPortState == SPSTATE_INITIALIZED_SERIAL) {
|
||||
if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
|
||||
bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
|
||||
|
||||
if (enableSerialTelemetry && !smartPortSerialPort) {
|
||||
|
|
@ -589,10 +587,7 @@ 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);
|
||||
|
|
@ -600,6 +595,6 @@ void handleSmartPortTelemetry(void)
|
|||
}
|
||||
|
||||
processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue