1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00

CF/BF - Fix HoTT telemetry.

Two issues:
1 - failure to work on softserial ports.

The TX pin was not allocated due to the initial port mode.

HoTT telemetry is different in that it changes the serial port mode
between sending and receiving.

This change opens the port in RX/TX mode so that both RX and TX pins are
initally allocated.

Test scenario:
* HoTT on SoftSerial 1
* Diode connecting between RX and TX pins.

2 - bidirectional hardware ports were not supported.

Renamed `sport_halfduplex` to `tlm_halfduplex`.  The setting is now used
by sport and hott telemetry.
This commit is contained in:
Hydra 2017-04-22 13:37:59 +01:00 committed by Dominic Clifton
parent 1d6c9382e4
commit 9726e52a54
8 changed files with 77 additions and 43 deletions

View file

@ -176,6 +176,7 @@ static const char * const gyroNames[] = {
static const char * const *sensorHardwareNames[] = {
gyroNames, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware
};
#endif // USE_SENSOR_NAMES

View file

@ -588,7 +588,7 @@ const clivalue_t valueTable[] = {
#ifdef TELEMETRY
{ "tlm_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_switch) },
{ "tlm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inversion) },
{ "sport_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, sportHalfDuplex) },
{ "tlm_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, halfDuplex) },
{ "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) },
{ "frsky_gps_format", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, FRSKY_FORMAT_NMEA }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_coordinate_format) },

View file

@ -56,6 +56,6 @@ void targetConfiguration(void)
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS;
telemetryConfigMutable()->telemetry_inversion = 1;
telemetryConfigMutable()->sportHalfDuplex = 1;
telemetryConfigMutable()->halfDuplex = 1;
}
#endif

View file

@ -54,6 +54,6 @@ void targetConfiguration(void)
serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So SPRacingF3OSD users don't have to change anything.
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
telemetryConfigMutable()->telemetry_inversion = 0;
telemetryConfigMutable()->sportHalfDuplex = 0;
telemetryConfigMutable()->halfDuplex = 0;
}
#endif

View file

@ -109,7 +109,7 @@ static uint8_t hottMsgCrc;
#define HOTT_CRC_SIZE (sizeof(hottMsgCrc))
#define HOTT_BAUDRATE 19200
#define HOTT_INITIAL_PORT_MODE MODE_RX
#define HOTT_PORT_MODE MODE_RXTX // must be opened in RXTX so that TX and RX pins are allocated.
static serialPort_t *hottPort = NULL;
static serialPortConfig_t *portConfig;
@ -320,18 +320,82 @@ void initHoTTTelemetry(void)
initialiseMessages();
}
static void flushHottRxBuffer(void)
{
while (serialRxBytesWaiting(hottPort) > 0) {
serialRead(hottPort);
}
}
static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) {
closeSerialPort(hottPort);
portOptions_t portOptions = SERIAL_NOT_INVERTED;
if (telemetryConfig()->halfDuplex) {
portOptions |= SERIAL_BIDIR;
}
hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, portOptions);
}
static bool hottIsUsingHardwareUART(void) {
return !(portConfig->identifier == SERIAL_PORT_SOFTSERIAL1 || portConfig->identifier == SERIAL_PORT_SOFTSERIAL2);
}
static void hottConfigurePortForTX(void) {
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
if (hottIsUsingHardwareUART())
workAroundForHottTelemetryOnUsart(hottPort, MODE_TX);
else
serialSetMode(hottPort, MODE_TX);
}
static void hottConfigurePortForRX(void) {
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
if (hottIsUsingHardwareUART())
workAroundForHottTelemetryOnUsart(hottPort, MODE_RX);
else
serialSetMode(hottPort, MODE_RX);
flushHottRxBuffer();
}
static void hottReconfigurePort(void) {
if (!hottIsSending) {
hottIsSending = true;
hottMsgCrc = 0;
hottConfigurePortForTX();
return;
}
if (hottMsgRemainingBytesToSendCount == 0) {
hottMsg = NULL;
hottIsSending = false;
hottConfigurePortForRX();
return;
}
}
void configureHoTTTelemetryPort(void)
{
if (!portConfig) {
return;
}
hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
portOptions_t portOptions = SERIAL_NOT_INVERTED;
if (telemetryConfig()->halfDuplex) {
portOptions |= SERIAL_BIDIR;
}
hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, HOTT_PORT_MODE, portOptions);
if (!hottPort) {
return;
}
hottConfigurePortForRX();
hottTelemetryEnabled = true;
}
@ -401,13 +465,6 @@ static void processBinaryModeRequest(uint8_t address) {
}
static void flushHottRxBuffer(void)
{
while (serialRxBytesWaiting(hottPort) > 0) {
serialRead(hottPort);
}
}
static void hottCheckSerialData(uint32_t currentMicros)
{
static bool lookingForRequest = true;
@ -452,34 +509,9 @@ static void hottCheckSerialData(uint32_t currentMicros)
}
}
static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) {
closeSerialPort(hottPort);
hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED);
}
static void hottSendTelemetryData(void) {
if (!hottIsSending) {
hottIsSending = true;
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
workAroundForHottTelemetryOnUsart(hottPort, MODE_TX);
else
serialSetMode(hottPort, MODE_TX);
hottMsgCrc = 0;
return;
}
if (hottMsgRemainingBytesToSendCount == 0) {
hottMsg = NULL;
hottIsSending = false;
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
workAroundForHottTelemetryOnUsart(hottPort, MODE_RX);
else
serialSetMode(hottPort, MODE_RX);
flushHottRxBuffer();
return;
}
hottReconfigurePort();
--hottMsgRemainingBytesToSendCount;
if(hottMsgRemainingBytesToSendCount == 0) {
@ -512,10 +544,11 @@ void checkHoTTTelemetryState(void)
return;
}
if (newTelemetryEnabledValue)
if (newTelemetryEnabledValue) {
configureHoTTTelemetryPort();
else
} else {
freeHoTTTelemetryPort();
}
}
void handleHoTTTelemetry(timeUs_t currentTimeUs)

View file

@ -323,7 +323,7 @@ void configureSmartPortTelemetryPort(void)
portOptions_t portOptions = 0;
if (telemetryConfig()->sportHalfDuplex) {
if (telemetryConfig()->halfDuplex) {
portOptions |= SERIAL_BIDIR;
}

View file

@ -64,7 +64,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY
PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.telemetry_inversion = TELEMETRY_DEFAULT_INVERSION,
.sportHalfDuplex = 1,
.halfDuplex = 1,
.telemetry_switch = 0,
.gpsNoFixLatitude = 0,
.gpsNoFixLongitude = 0,

View file

@ -42,7 +42,7 @@ typedef struct telemetryConfig_s {
int16_t gpsNoFixLongitude;
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
uint8_t telemetry_inversion; // also shared with smartport inversion
uint8_t sportHalfDuplex;
uint8_t halfDuplex;
frskyGpsCoordFormat_e frsky_coordinate_format;
frskyUnit_e frsky_unit;
uint8_t frsky_vfas_precision;