chore: New module/serial API (#3055)

* WiP: new module interface

* WiP: softserial timer DMA

* WiP: setup() and send() in one step

and remove methods specific to internal or external module

* fixes and extra port config

* horus targets

* module ports have moved

* T8 support (only internal module)

* fix nv14

* stupid GCC....

* disable BT

* remove nv14 telem driver

* remove old aux serial driver

* Move module buffers out of pulses.h

* Soft serial fixes and PXX1 support

* Fixed PPM module output

* Fixed CLI

* compilation fixes

* Ported bluetooth and flysky gimbal to new serial driver

* Re-enable soft-serial RX

* fixes and cleanup

* nv14 fixes

* Fix flysky gimbals

* Ghost module

* 16 bytes of RAM

* Remove getByte from module API

* Ghost cleanup

* fix: _half_duplex_input() must be called after usart init

* WiP

* more cleanup

* Remove board specific settings from PXX1 / PXX2 drivers

* A bit of PXX2 header cleanup

* Add support for OVER8 and ONEBIT

* Add some support for inverters (NV14 / EL18)

* Support NV14 power fixes in generic board

* Fix polarity handling

* Fix RX DMA

* Fix mixer not running

* Add soft-serial polarity control

* Quick fix for telemetry polling while module port is changed

* Fix bootloader DEBUG

* Fix MPM telemetry

* Fix pulse start

* DSM2 soft-serial fallback

* DSMP async module restart

* Fix CRSF LUA upstream

* Remove global variable telemetryProtocol

* Use HAL/LL constants

* Fix various issue with AFHDS3 and NV14/EL18 internal module

* Fixed S.PORT unit tests

* fix: update widx after writing at current index

* fix: init error condition

* Fix: possible infinite loop in _find_port()

* Add support for bidir S.PORT init

* Add support for "getLastByte" in HW serial driver

* PXX1: Add support for S.PORT based on IDLE callback

* Fix rebase error

* Fix softserial _rx_fifo_push()

* Add softserial support for waitForTxCompleted()

* Fix MPM flashing (internal / external)

* Fixed external MPM

* Activate RX DMA on internal module

Seems to work with serial passthrough without issues, at least on the L3R PRO.

* Fix softserial DMA on stdlcd F4 targets

* Add softserial support for 32bit timers (mostly TIM2)

* More precise soft-serial

* Fix MPM update using inverters & HW uart

* Raise S.PORT DMA/USART IRQ prio to achieve faster switching to input

* Remove useless delays

* Fix flashing external ELRS module

* Added a retry mechanism for FrSky S.PORT update

* Remove useless enableRx()

* External FrSky modules are all flashed via S.PORT

* Remove power management unit update

This was only used on the first batch of X-lite.
This commit is contained in:
Raphael Coeffic
2023-02-09 19:44:32 +10:00
committed by GitHub
parent d80adc6c4d
commit 72193eda82
189 changed files with 7960 additions and 8364 deletions
+6 -7
View File
@@ -22,7 +22,6 @@
#include "crossfire.h"
#include "opentx.h"
#include "aux_serial_driver.h"
const CrossfireSensor crossfireSensors[] = {
{LINK_ID, 0, STR_SENSOR_RX_RSSI1, UNIT_DB, 0},
@@ -62,19 +61,19 @@ const CrossfireSensor crossfireSensors[] = {
const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
{
if (id == LINK_ID)
return crossfireSensors[RX_RSSI1_INDEX+subId];
return crossfireSensors[RX_RSSI1_INDEX + subId];
else if (id == LINK_RX_ID)
return crossfireSensors[RX_RSSI_PERC_INDEX+subId];
return crossfireSensors[RX_RSSI_PERC_INDEX + subId];
else if (id == LINK_TX_ID)
return crossfireSensors[TX_RSSI_PERC_INDEX+subId];
return crossfireSensors[TX_RSSI_PERC_INDEX + subId];
else if (id == BATTERY_ID)
return crossfireSensors[BATT_VOLTAGE_INDEX+subId];
return crossfireSensors[BATT_VOLTAGE_INDEX + subId];
else if (id == GPS_ID)
return crossfireSensors[GPS_LATITUDE_INDEX+subId];
return crossfireSensors[GPS_LATITUDE_INDEX + subId];
else if (id == CF_VARIO_ID)
return crossfireSensors[VERTICAL_SPEED_INDEX];
else if (id == ATTITUDE_ID)
return crossfireSensors[ATTITUDE_PITCH_INDEX+subId];
return crossfireSensors[ATTITUDE_PITCH_INDEX + subId];
else if (id == FLIGHT_MODE_ID)
return crossfireSensors[FLIGHT_MODE_INDEX];
else if (id == BARO_ALT_ID)
+20 -3
View File
@@ -141,9 +141,26 @@ const uint8_t CROSSFIRE_FRAME_PERIODS[] = {
% DIM(CROSSFIRE_BAUDRATES)
#endif
#define INT_CROSSFIRE_BAUDRATE CROSSFIRE_BAUDRATES[CROSSFIRE_STORE_TO_INDEX(g_eeGeneral.internalModuleBaudrate)]
#define EXT_CROSSFIRE_BAUDRATE CROSSFIRE_BAUDRATES[CROSSFIRE_STORE_TO_INDEX(g_model.moduleData[EXTERNAL_MODULE].crsf.telemetryBaudrate)]
#define CROSSFIRE_PERIOD (CROSSFIRE_FRAME_PERIODS[CROSSFIRE_STORE_TO_INDEX(g_model.moduleData[EXTERNAL_MODULE].crsf.telemetryBaudrate)] * 1000)
#if defined(HARDWARE_INTERNAL_MODULE)
#define INT_CROSSFIRE_BR_IDX CROSSFIRE_STORE_TO_INDEX(g_eeGeneral.internalModuleBaudrate)
#define INT_CROSSFIRE_BAUDRATE CROSSFIRE_BAUDRATES[INT_CROSSFIRE_BR_IDX]
#define INT_CROSSFIRE_PERIOD (CROSSFIRE_FRAME_PERIODS[INT_CROSSFIRE_BR_IDX] * 1000)
#endif
#if defined(HARDWARE_EXTERNAL_MODULE)
#define EXT_CROSSFIRE_BR_IDX CROSSFIRE_STORE_TO_INDEX(g_model.moduleData[EXTERNAL_MODULE].crsf.telemetryBaudrate)
#define EXT_CROSSFIRE_BAUDRATE CROSSFIRE_BAUDRATES[EXT_CROSSFIRE_BR_IDX]
#define EXT_CROSSFIRE_PERIOD (CROSSFIRE_FRAME_PERIODS[EXT_CROSSFIRE_BR_IDX] * 1000)
#endif
#if defined(HARDWARE_INTERNAL_MODULE) && defined(HARDWARE_EXTERNAL_MODULE)
#define CROSSFIRE_PERIOD(module) \
(module == INTERNAL_MODULE ? INT_CROSSFIRE_PERIOD : EXT_CROSSFIRE_PERIOD)
#elif defined(HARDWARE_INTERNAL_MODULE)
#define CROSSFIRE_PERIOD(module) INT_CROSSFIRE_PERIOD
#elif defined(HARDWARE_EXTERNAL_MODULE)
#define CROSSFIRE_PERIOD(module) EXT_CROSSFIRE_PERIOD
#endif
#define CROSSFIRE_TELEM_MIRROR_BAUDRATE 115200
+1
View File
@@ -22,6 +22,7 @@
#include "opentx.h"
#include "flysky_nv14.h"
#include "flysky_ibus.h"
#include "pulses/afhds2.h"
#include "dataconstants.h"
+27 -28
View File
@@ -20,35 +20,23 @@
*/
#include "opentx.h"
#include "aux_serial_driver.h"
void processFrskyTelemetryData(uint8_t data)
{
if (pushFrskyTelemetryData(data)) {
if (IS_FRSKY_SPORT_PROTOCOL()) {
sportProcessTelemetryPacket(telemetryRxBuffer);
}
else {
frskyDProcessPacket(telemetryRxBuffer);
}
}
}
bool pushFrskyTelemetryData(uint8_t data)
static inline bool pushFrskyTelemetryData(bool is_sport, uint8_t data,
uint8_t* buffer, uint8_t& len)
{
static uint8_t dataState = STATE_DATA_IDLE;
switch (dataState) {
case STATE_DATA_START:
if (data == START_STOP) {
if (IS_FRSKY_SPORT_PROTOCOL()) {
if (is_sport) {
dataState = STATE_DATA_IN_FRAME ;
telemetryRxBufferCount = 0;
len = 0;
}
}
else {
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (len < TELEMETRY_RX_PACKET_SIZE) {
buffer[len++] = data;
}
dataState = STATE_DATA_IN_FRAME;
}
@@ -59,39 +47,38 @@ bool pushFrskyTelemetryData(uint8_t data)
dataState = STATE_DATA_XOR; // XOR next byte
}
else if (data == START_STOP) {
if (IS_FRSKY_SPORT_PROTOCOL()) {
if (is_sport) {
dataState = STATE_DATA_IN_FRAME ;
telemetryRxBufferCount = 0;
}
else {
len = 0;
} else {
// end of frame detected
dataState = STATE_DATA_IDLE;
return true;
}
break;
}
else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
else if (len < TELEMETRY_RX_PACKET_SIZE) {
buffer[len++] = data;
}
break;
case STATE_DATA_XOR:
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data ^ STUFF_MASK;
if (len < TELEMETRY_RX_PACKET_SIZE) {
buffer[len++] = data ^ STUFF_MASK;
}
dataState = STATE_DATA_IN_FRAME;
break;
case STATE_DATA_IDLE:
if (data == START_STOP) {
telemetryRxBufferCount = 0;
len = 0;
dataState = STATE_DATA_START;
}
break;
} // switch
if (IS_FRSKY_SPORT_PROTOCOL() && telemetryRxBufferCount >= FRSKY_SPORT_PACKET_SIZE) {
if (is_sport && len >= FRSKY_SPORT_PACKET_SIZE) {
// end of frame detected
dataState = STATE_DATA_IDLE;
return true;
@@ -100,4 +87,16 @@ bool pushFrskyTelemetryData(uint8_t data)
return false;
}
void processFrskyDTelemetryData(uint8_t module, uint8_t data, uint8_t* buffer, uint8_t& len)
{
if (pushFrskyTelemetryData(false, data, buffer, len)) {
frskyDProcessPacket(module, buffer, len);
}
}
void processFrskySportTelemetryData(uint8_t module, uint8_t data, uint8_t* buffer, uint8_t& len)
{
if (pushFrskyTelemetryData(true, data, buffer, len)) {
sportProcessTelemetryPacket(module, buffer, len);
}
}
+10 -5
View File
@@ -95,16 +95,21 @@ typedef enum {
// FrSky D Telemetry Protocol
void processHubPacket(uint8_t id, int16_t value);
void frskyDProcessPacket(const uint8_t *packet);
void frskyDProcessPacket(uint8_t module, const uint8_t *packet, uint8_t len);
// FrSky S.PORT Telemetry Protocol
bool sportProcessTelemetryPacket(const uint8_t * packet);
bool sportProcessTelemetryPacket(uint8_t module, const uint8_t * packet, uint8_t len);
void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance,
uint32_t data, TelemetryUnit unit = UNIT_RAW);
void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t *packet);
bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected
void processFrskyTelemetryData(uint8_t data);
void sportProcessTelemetryPacketWithoutCrc(uint8_t module, uint8_t origin,
const uint8_t *packet);
void processFrskySportTelemetryData(uint8_t module, uint8_t data,
uint8_t* buffer, uint8_t& len);
void processFrskyDTelemetryData(uint8_t module, uint8_t data,
uint8_t* buffer, uint8_t& len);
#if defined(NO_RAS)
inline bool isRasValueValid()
+21 -15
View File
@@ -62,31 +62,37 @@ void parseTelemHubByte(uint8_t byte)
processHubPacket(structPos, (byte << 8) + lowByte);
}
void frskyDProcessPacket(const uint8_t *packet)
void frskyDProcessPacket(uint8_t module, const uint8_t *packet, uint8_t len)
{
const TelemetryProtocol proto = PROTOCOL_TELEMETRY_FRSKY_D;
// What type of packet?
switch (packet[0])
{
case LINKPKT: // A1/A2/RSSI values
// A1/A2/RSSI values
case LINKPKT:
{
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, D_A1_ID, 0, 0, packet[1], UNIT_VOLTS, 0);
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, D_A2_ID, 0, 0, packet[2], UNIT_VOLTS, 0);
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, D_RSSI_ID, 0, 0, packet[3], UNIT_RAW, 0);
#if defined(MULTIMODULE)
if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, TX_RSSI_ID, 0, 0, packet[4]>>1, UNIT_DB, 0);
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, RX_LQI_ID, 0, 0, packet[5] , UNIT_RAW, 0);
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, TX_LQI_ID , 0, 0, packet[6] , UNIT_RAW, 0);
setTelemetryValue(proto, D_A1_ID, 0, 0, packet[1], UNIT_VOLTS, 0);
setTelemetryValue(proto, D_A2_ID, 0, 0, packet[2], UNIT_VOLTS, 0);
setTelemetryValue(proto, D_RSSI_ID, 0, 0, packet[3], UNIT_RAW, 0);
if (len >= 7) {
setTelemetryValue(proto, TX_RSSI_ID, 0, 0, packet[4]>>1, UNIT_DB, 0);
setTelemetryValue(proto, RX_LQI_ID, 0, 0, packet[5] , UNIT_RAW, 0);
setTelemetryValue(proto, TX_LQI_ID , 0, 0, packet[6] , UNIT_RAW, 0);
}
#endif
// reset counter only if valid packets are being detected
telemetryData.rssi.set(packet[3]);
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
break;
}
case USRPKT: // User Data packet
uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
for (uint8_t i=3; i<numBytes; i++) {
// User Data packet
case USRPKT:
// sanitize in case of data corruption
// leading to buffer overflow
uint8_t numBytes = 3 + (packet[1] & 0x07);
for (uint8_t i = 3; i < numBytes; i++) {
parseTelemHubByte(packet[i]);
}
break;
+78 -14
View File
@@ -21,16 +21,84 @@
#include "opentx.h"
#if defined(PXX2)
#if defined(LIBOPENUI)
#include "libopenui.h"
#endif
#if defined(INTMODULE_USART)
#include "intmodule_serial_driver.h"
#endif
#include "pulses/pxx2.h"
#include "pulses/pxx2_transport.h"
static_assert(PXX2_FRAME_MAXLENGTH <= INTMODULE_FIFO_SIZE, "");
static const char * const PXX2ModulesNames[] = {
"---",
"XJT",
"ISRM",
"ISRM-PRO",
"ISRM-S",
"R9M",
"R9MLite",
"R9MLite-PRO",
"ISRM-N",
"ISRM-S-X9",
"ISRM-S-X10E",
"XJT Lite",
"ISRM-S-X10S",
"ISRM-X9LiteS"
};
const char * getPXX2ModuleName(uint8_t modelId)
{
if (modelId < DIM(PXX2ModulesNames))
return PXX2ModulesNames[modelId];
else
return PXX2ModulesNames[0];
}
static const char * const PXX2ReceiversNames[] = {
"---",
"X8R",
"RX8R",
"RX8R-PRO",
"RX6R",
"RX4R",
"G-RX8",
"G-RX6",
"X6R",
"X4R",
"X4R-SB",
"XSR",
"XSR-M",
"RXSR",
"S6R",
"S8R",
"XM",
"XM+",
"XMR",
"R9",
"R9-SLIM",
"R9-SLIM+",
"R9-MINI",
"R9-MM",
"R9-STAB", // R9-STAB has OTA
"R9-MINI-OTA", // this one has OTA (different bootloader)
"R9-MM-OTA", // this one has OTA (different bootloader)
"R9-SLIM+-OTA", // this one has OTA (different bootloader)
"Archer-X", // this one has OTA (internal module)
"R9MX", // this one has OTA
"R9SX", // this one has OTA
};
const char * getPXX2ReceiverName(uint8_t modelId)
{
if (modelId < DIM(PXX2ReceiversNames))
return PXX2ReceiversNames[modelId];
else
return PXX2ReceiversNames[0];
}
static void processGetHardwareInfoFrame(uint8_t module, const uint8_t * frame)
{
if (moduleState[module].mode != MODULE_MODE_GET_HARDWARE_INFO) {
@@ -216,14 +284,8 @@ static void processResetFrame(uint8_t module, const uint8_t * frame)
static void processTelemetryFrame(uint8_t module, const uint8_t * frame)
{
for (uint8_t i = 0; i < 1 + frame[0]; i++) {
telemetryMirrorSend(frame[i]);
}
uint8_t origin = (module << 2) + (frame[3] & 0x03);
if (origin != TELEMETRY_ENDPOINT_SPORT) {
sportProcessTelemetryPacketWithoutCrc(origin, &frame[4]);
}
uint8_t origin = frame[3] & 0x03;
sportProcessTelemetryPacketWithoutCrc(module, origin, &frame[4]);
}
#if defined(INTERNAL_MODULE_PXX2) && defined(ACCESS_DENIED) && !defined(SIMU)
@@ -254,9 +316,10 @@ static void processAuthenticationFrame(uint8_t module, const uint8_t * frame,
moduleState[module].mode = MODULE_MODE_AUTHENTICATION;
Pxx2Pulses &pxx2 = intmodulePulsesData.pxx2;
pxx2.setupAuthenticationFrame(module, cryptoType, (const uint8_t *)messageDigest);
drv->sendBuffer(ctx, pxx2.getData(), pxx2.getSize());
uint8_t* module_buffer = pulsesGetModuleBuffer(module);
Pxx2Pulses pxx2(module_buffer);
pxx2.setupAuthenticationFrame(module, cryptoType, (const uint8_t *)messageDigest);
drv->sendBuffer(ctx, module_buffer, pxx2.getSize());
// we remain in AUTHENTICATION mode to avoid a CHANNELS frame is sent at the
// end of the mixing process
@@ -417,3 +480,4 @@ void processPXX2Frame(uint8_t module, const uint8_t * frame,
}
}
#endif
+114 -41
View File
@@ -68,8 +68,8 @@ const FrSkySportSensor sportSensors[] = {
{ RBOX_BATT2_FIRST_ID, RBOX_BATT2_LAST_ID, 1, STR_SENSOR_BATT2_CURRENT, UNIT_AMPS, 2 },
{ RBOX_CNSP_FIRST_ID, RBOX_CNSP_LAST_ID, 0, STR_SENSOR_BATT1_CONSUMPTION, UNIT_MAH, 0 },
{ RBOX_CNSP_FIRST_ID, RBOX_CNSP_LAST_ID, 1, STR_SENSOR_BATT2_CONSUMPTION, UNIT_MAH, 0 },
{ RBOX_STATE_FIRST_ID, RBOX_STATE_LAST_ID, 0, STR_SENSOR_CHANS_STATE, UNIT_BITFIELD, 0 },
{ RBOX_STATE_FIRST_ID, RBOX_STATE_LAST_ID, 1, STR_SENSOR_RB_STATE, UNIT_BITFIELD, 0 },
{ RBOX_STATE_FIRST_ID, RBOX_STATE_LAST_ID, 0, STR_SENSOR_CHANS_STATE, UNIT_TEXT, 0 },
{ RBOX_STATE_FIRST_ID, RBOX_STATE_LAST_ID, 1, STR_SENSOR_RB_STATE, UNIT_TEXT, 0 },
{ SD1_FIRST_ID, SD1_LAST_ID, 0, STR_SENSOR_SD1_CHANNEL, UNIT_RAW, 0 },
{ ESC_POWER_FIRST_ID, ESC_POWER_LAST_ID, 0, STR_SENSOR_ESC_VOLTAGE, UNIT_VOLTS, 2 },
{ ESC_POWER_FIRST_ID, ESC_POWER_LAST_ID, 1, STR_SENSOR_ESC_CURRENT, UNIT_AMPS, 2 },
@@ -86,7 +86,7 @@ const FrSkySportSensor sportSensors[] = {
{ GASSUIT_AVG_FLOW_FIRST_ID, GASSUIT_AVG_FLOW_LAST_ID, 0, STR_SENSOR_GASSUIT_AVG_FLOW, UNIT_MILLILITERS_PER_MINUTE, 0 },
{ SBEC_POWER_FIRST_ID, SBEC_POWER_LAST_ID, 0, STR_SENSOR_SBEC_VOLTAGE, UNIT_VOLTS, 2 },
{ SBEC_POWER_FIRST_ID, SBEC_POWER_LAST_ID, 1, STR_SENSOR_SBEC_CURRENT, UNIT_AMPS, 2 },
{ RB3040_OUTPUT_FIRST_ID, RB3040_OUTPUT_LAST_ID, 0, STR_RB3040_EXTRA_STATE, UNIT_BITFIELD, 0 },
{ RB3040_OUTPUT_FIRST_ID, RB3040_OUTPUT_LAST_ID, 0, STR_RB3040_EXTRA_STATE, UNIT_TEXT, 0 },
{ RB3040_CH1_2_FIRST_ID, RB3040_CH1_2_LAST_ID, 0, STR_RB3040_CHANNEL1, UNIT_AMPS, 2 },
{ RB3040_CH1_2_FIRST_ID, RB3040_CH1_2_LAST_ID, 1, STR_RB3040_CHANNEL2, UNIT_AMPS, 2 },
{ RB3040_CH3_4_FIRST_ID, RB3040_CH3_4_LAST_ID, 0, STR_RB3040_CHANNEL3, UNIT_AMPS, 2 },
@@ -135,31 +135,34 @@ uint16_t rboxState;
void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance,
uint32_t data, TelemetryUnit unit)
{
const TelemetryProtocol proto = PROTOCOL_TELEMETRY_FRSKY_SPORT;
const FrSkySportSensor * sensor = getFrSkySportSensor(id, subId);
uint8_t precision = 255;
if (sensor) {
if (unit == UNIT_RAW)
if (unit == UNIT_RAW) {
unit = sensor->unit;
}
precision = sensor->prec;
}
if (unit == UNIT_CELLS) {
uint8_t cellsCount = (data & 0xF0u) >> 4u;
uint8_t cellIndex = (data & 0x0Fu);
if (cellIndex >= MAX_CELLS)
return;
if (cellIndex >= MAX_CELLS) return;
uint32_t mask = (cellsCount << 24u) + (cellIndex << 16u);
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_SPORT, id, subId, instance, mask + (((data & 0x000FFF00u) >> 8u) / 5), unit, precision);
uint32_t value = mask + (((data & 0x000FFF00u) >> 8u) / 5);
setTelemetryValue(proto, id, subId, instance, value, unit, precision);
if (cellIndex + 1 < cellsCount) {
mask += (1 << 16u);
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_SPORT, id, subId, instance, mask + (((data & 0xFFF00000u) >> 20u) / 5), unit, precision);
value = mask + (((data & 0xFFF00000u) >> 20u) / 5);
setTelemetryValue(proto, id, subId, instance, value, unit, precision);
}
}
else {
setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_SPORT, id, subId, instance, data, unit, precision);
setTelemetryValue(proto, id, subId, instance, data, unit, precision);
}
}
bool sportProcessTelemetryPacket(const uint8_t * packet)
bool sportProcessTelemetryPacket(uint8_t module, const uint8_t * packet, uint8_t len)
{
if (!checkSportPacket(packet)) {
TRACE("sportProcessTelemetryPacket(): checksum error ");
@@ -167,11 +170,85 @@ bool sportProcessTelemetryPacket(const uint8_t * packet)
return false;
}
sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet);
sportProcessTelemetryPacketWithoutCrc(module, 0, packet);
return true;
}
void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet)
static void sportSetServoState(uint16_t id, uint8_t subId, uint8_t instance, uint16_t value)
{
const TelemetryProtocol proto = PROTOCOL_TELEMETRY_FRSKY_SPORT;
if (value == 0) {
setTelemetryText(proto, id, subId, instance, "OK");
}
else {
for (uint8_t i = 0; i < 16; i++) {
if (value & (1u << i)) {
char s[] = "CH__ KO";
strAppendUnsigned(&s[2], i + 1, 2);
setTelemetryText(proto, id, subId, instance, s);
break;
}
}
}
}
static void sportSetRBoxState(uint16_t id, uint8_t subId, uint8_t instance, uint16_t value)
{
const TelemetryProtocol proto = PROTOCOL_TELEMETRY_FRSKY_SPORT;
if (value == 0) {
setTelemetryText(proto, id, subId, instance, "Rx OK");
}
else {
static const char * const RXS_STATUS[] = {
"Rx1 Ovl",
"Rx2 Ovl",
"SBUS Ovl",
"Rx1 FS",
"Rx1 LF",
"Rx2 FS",
"Rx2 LF",
"Rx1 Lost",
"Rx2 Lost",
"Rx1 NS",
"Rx2 NS",
"Rx3 FS",
"Rx3 LF",
"Rx3 Lost",
"Rx3 NS"
};
for (uint8_t i = 0; i < DIM(RXS_STATUS); i++) {
if (value & (1u << i)) {
setTelemetryText(proto, id, subId, instance, RXS_STATUS[i]);
break;
}
}
}
}
static void sportSetRB3040State(uint16_t id, uint8_t subId, uint8_t instance, uint32_t value)
{
const TelemetryProtocol proto = PROTOCOL_TELEMETRY_FRSKY_SPORT;
if (value == 0) {
setTelemetryText(proto, id, subId, instance, "OK");
}
else {
for (uint8_t i = 0; i < 9; i++) {
if (value & (1u << i)) {
if (i < 8) {
char s[] = "CH__ KO";
strAppendUnsigned(&s[2], i + 17, 2);
setTelemetryText(proto, id, subId, instance, s);
break;
} else {
setTelemetryText(proto, id, subId, instance, "S.P Ovl");
break;
}
}
}
}
}
void sportProcessTelemetryPacketWithoutCrc(uint8_t module, uint8_t origin, const uint8_t * packet)
{
uint8_t physicalId = packet[0] & 0x1F;
uint8_t primId = packet[1];
@@ -179,29 +256,14 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
uint32_t data = SPORT_DATA_S32(packet);
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.forwardTelemetry(packet);
}
#endif
if (primId == DATA_FRAME) {
uint8_t originMask;
uint8_t moduleIndex;
if (origin == TELEMETRY_ENDPOINT_SPORT) {
#if defined(SIMU) && defined(INTERNAL_MODULE_PXX2)
// When running simu on ACCESS radio, we set the origin as internal module
moduleIndex = 0;
origin = 0;
originMask = 0x01;
#else
moduleIndex = isSportLineUsedByInternalModule() ? 0 : 1;
originMask = 0x04;
#endif
}
else {
moduleIndex = (origin >> 2);
originMask = 0x01 << moduleIndex;
}
uint8_t originMask = 0x01 << module;
uint8_t instance = physicalId + (origin << 5);
if (dataId == RSSI_ID) {
data = SPORT_DATA_U8(packet);
@@ -230,7 +292,9 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
}
else if (dataId == R9_PWR_ID) {
// convert 'dBm' to 'mW'
static const uint32_t r9pwrs[][2] = {{0, 1}, {5, 3}, {10, 10}, {13, 20}, {14, 25}, {20, 100}, {23, 200}, {27, 500}, {30, 1000}};
static const uint32_t r9pwrs[][2] = {{0, 1}, {5, 3}, {10, 10},
{13, 20}, {14, 25}, {20, 100},
{23, 200}, {27, 500}, {30, 1000}};
uint32_t dBm = SPORT_DATA_U8(packet);
for (auto & r9pwr : r9pwrs) {
if (dBm == r9pwr[0]) {
@@ -242,22 +306,25 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
else if (dataId == XJT_VERSION_ID) {
telemetryData.xjtVersion = HUB_DATA_U16(packet);
if (!isRasValueValid()) {
telemetryData.setSwr(moduleIndex, 0);
telemetryData.setSwr(module, 0);
}
}
else if (dataId == RAS_ID) {
if (isRasValueValid()) {
telemetryData.setSwr(moduleIndex, SPORT_DATA_U8(packet));
telemetryData.setSwr(module, SPORT_DATA_U8(packet));
}
}
// here we discard the frame if it comes from an origin which has RSSI = 0 (RxBt and RSSI are sent in a loop by the module in some situations)
if (TELEMETRY_STREAMING() && (telemetryData.telemetryValid & originMask)/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
// here we discard the frame if it comes from an origin which has RSSI = 0
// (RxBt and RSSI are sent in a loop by the module in some situations)
if (TELEMETRY_STREAMING() &&
// because when Rx is OFF it happens that some
// old A1/A2 values are sent from the XJT module
(telemetryData.telemetryValid & originMask)) {
if ((dataId >> 8) == 0) {
// The old FrSky IDs
processHubPacket(dataId, HUB_DATA_U16(packet));
}
else if (!IS_HIDDEN_TELEMETRY_VALUE(dataId)) {
} else if (!IS_HIDDEN_TELEMETRY_VALUE(dataId)) {
if (dataId == ADC1_ID || dataId == ADC2_ID || dataId == BATT_ID || dataId == RAS_ID) {
data = SPORT_DATA_U8(packet);
}
@@ -302,8 +369,11 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
}
servosState = newServosState;
rboxState = newRboxState;
sportProcessTelemetryPacket(dataId, 0, instance, servosState);
sportProcessTelemetryPacket(dataId, 1, instance, rboxState);
sportSetServoState(dataId, 0, instance, servosState);
sportSetRBoxState(dataId, 1, instance, rboxState);
}
else if (dataId >= RB3040_OUTPUT_FIRST_ID && dataId <= RB3040_OUTPUT_LAST_ID) {
sportSetRB3040State(dataId, 0, instance, data);
}
else if (dataId >= ESC_POWER_FIRST_ID && dataId <= ESC_POWER_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0xFFFFu);
@@ -322,7 +392,9 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
}
else if (dataId >= DIY_STREAM_FIRST_ID && dataId <= DIY_STREAM_LAST_ID) {
#if defined(LUA)
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) {
if (luaInputTelemetryFifo &&
luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) {
SportTelemetryPacket luaPacket;
luaPacket.physicalId = physicalId;
luaPacket.primId = primId;
@@ -343,7 +415,8 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
sportProcessTelemetryPacket(dataId, 1, instance, (data >> 8u) & 0xFFu);
sportProcessTelemetryPacket(dataId, 2, instance, (data >> 16u) & 0xFFu);
uint8_t newServosState = data >> 24u;
setTelemetryText(PROTOCOL_TELEMETRY_FRSKY_SPORT, dataId, 3, instance, newServosState ? "STALL" : "OK");
setTelemetryText(PROTOCOL_TELEMETRY_FRSKY_SPORT, dataId, 3, instance,
newServosState ? "STALL" : "OK");
if (newServosState != 0 && servosState == 0) {
audioEvent(AU_SERVO_KO);
servosState = newServosState;
+60 -87
View File
@@ -23,7 +23,6 @@
#include "ghost_menu.h"
#include "opentx.h"
#include "aux_serial_driver.h"
const char *ghstRfProfileValue[GHST_RF_PROFILE_COUNT] = { "Auto", "Norm", "Race", "Pure", "Long", "Unused", "Race2", "Pure2" };
const char *ghstVtxBandName[GHST_VTX_BAND_COUNT] = { "- - -" , "IRC", "Race", "BandE", "BandB", "BandA" };
@@ -92,14 +91,6 @@ const GhostSensor ghostSensors[] = {
{0x00, NULL, UNIT_RAW, 0},
};
uint8_t getGhostModuleAddr() {
#if SPORT_MAX_BAUDRATE < 400000
return g_model.moduleData[EXTERNAL_MODULE].ghost.telemetryBaudrate == GHST_TELEMETRY_RATE_400K ? GHST_ADDR_MODULE_SYM : GHST_ADDR_MODULE_ASYM;
#else
return GHST_ADDR_MODULE_SYM;
#endif
}
const GhostSensor *getGhostSensor(uint8_t id)
{
for (const GhostSensor * sensor = ghostSensors; sensor->id; sensor++) {
@@ -132,75 +123,78 @@ void processGhostTelemetryValueString(const GhostSensor * sensor, const char * s
}
}
bool checkGhostTelemetryFrameCRC()
static bool checkGhostTelemetryFrameCRC(const uint8_t* frame, uint32_t len)
{
uint8_t len = telemetryRxBuffer[1];
uint8_t crc = crc8(&telemetryRxBuffer[2], len - 1);
return (crc == telemetryRxBuffer[len + 1]);
uint8_t crc = crc8(frame, len - 1);
return (crc == frame[len - 1]);
}
// hifirst
uint16_t getTelemetryValue_u16(uint8_t index)
static uint16_t _get_u16(const uint8_t* frame, uint8_t offset)
{
return (telemetryRxBuffer[index] << 8) | telemetryRxBuffer[index + 1];
return (frame[offset] << 8) | frame[offset + 1];
}
// lofirst
uint16_t getTelemetryValue_u16le(uint8_t index)
static uint16_t _get_u16le(const uint8_t* frame, uint8_t offset)
{
return (telemetryRxBuffer[index + 1] << 8) | telemetryRxBuffer[index];
return (frame[offset + 1] << 8) | frame[offset];
}
// hifirst
uint32_t getTelemetryValue_s32(uint8_t index)
static uint32_t _get_s32(const uint8_t* frame, uint8_t offset)
{
uint32_t val = 0;
for (int i = 0; i < 4; ++i)
val <<= 8, val |= telemetryRxBuffer[index + i];
val <<= 8, val |= frame[offset + i];
return val;
}
// lofirst
uint32_t getTelemetryValue_s32le(uint8_t index)
static uint32_t _get_s32le(const uint8_t* frame, uint8_t offset)
{
uint32_t val = 0;
for (int i = 0; i < 4; ++i)
val <<= 8, val |= telemetryRxBuffer[index + 3 - i];
val <<= 8, val |= frame[offset + 3 - i];
return val;
}
void processGhostTelemetryFrame()
void processGhostTelemetryFrame(uint8_t module, uint8_t* buffer, uint32_t length)
{
if (!checkGhostTelemetryFrameCRC()) {
uint8_t frame_len = buffer[1];
auto frame = buffer + 2;
if (!checkGhostTelemetryFrameCRC(frame, frame_len)) {
TRACE("[GS] CRC error");
return;
}
uint8_t id = telemetryRxBuffer[2];
uint8_t id = frame[0];
switch(id) {
case GHST_DL_OPENTX_SYNC:
{
uint32_t update_interval = getTelemetryValue_s32(3);
int32_t offset = getTelemetryValue_s32(7);
uint32_t update_interval = _get_s32(frame, 1);
int32_t offset = _get_s32(frame, 5);
// values are in units of 100ns
update_interval /= 10;
offset /= 10;
getModuleSyncStatus(EXTERNAL_MODULE).update(update_interval, offset);
getModuleSyncStatus(module).update(update_interval, offset);
}
break;
case GHST_DL_LINK_STAT:
{
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount);
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(buffer, length);
}
#endif
uint8_t rssiVal = min<uint8_t>(telemetryRxBuffer[3], 120); // RSSI is a negative value, but sent as a positive integer.
uint8_t lqVal = min<uint8_t>(telemetryRxBuffer[4], 100);
uint8_t snrVal = min<uint8_t>(telemetryRxBuffer[5], 100);
// RSSI is a negative value, but sent as a positive integer.
uint8_t rssiVal = min<uint8_t>(frame[1], 120);
uint8_t lqVal = min<uint8_t>(frame[2], 100);
uint8_t snrVal = min<uint8_t>(frame[3], 100);
processGhostTelemetryValue(GHOST_ID_RX_RSSI, - rssiVal);
processGhostTelemetryValue(GHOST_ID_RX_LQ, lqVal);
@@ -216,10 +210,10 @@ void processGhostTelemetryFrame()
telemetryStreaming = 0;
}
processGhostTelemetryValue(GHOST_ID_TX_POWER, getTelemetryValue_u16(6));
processGhostTelemetryValue(GHOST_ID_FRAME_RATE, getTelemetryValue_u16(8));
processGhostTelemetryValue(GHOST_ID_TOTAL_LATENCY, getTelemetryValue_u16(10));
uint8_t rfModeEnum = min<uint8_t>(telemetryRxBuffer[12], GHST_RF_PROFILE_MAX);
processGhostTelemetryValue(GHOST_ID_TX_POWER, _get_u16(frame, 4));
processGhostTelemetryValue(GHOST_ID_FRAME_RATE, _get_u16(frame, 6));
processGhostTelemetryValue(GHOST_ID_TOTAL_LATENCY, _get_u16(frame, 8));
uint8_t rfModeEnum = min<uint8_t>(frame[10], GHST_RF_PROFILE_MAX);
// RF mode string, one char at a time
const GhostSensor * sensor = getGhostSensor(GHOST_ID_RF_MODE);
@@ -231,18 +225,19 @@ void processGhostTelemetryFrame()
case GHST_DL_VTX_STAT:
{
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount);
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(buffer, length);
}
#endif
uint8_t vtxBandEnum = min<uint8_t>(telemetryRxBuffer[8], GHST_VTX_BAND_MAX);
uint8_t vtxBandEnum = min<uint8_t>(frame[6], GHST_VTX_BAND_MAX);
const GhostSensor * sensor = getGhostSensor(GHOST_ID_VTX_BAND);
const char * vtxBandString = ghstVtxBandName[vtxBandEnum];
processGhostTelemetryValue(GHOST_ID_VTX_FREQ, getTelemetryValue_u16(4));
processGhostTelemetryValue(GHOST_ID_VTX_POWER, getTelemetryValue_u16(6));
processGhostTelemetryValue(GHOST_ID_VTX_CHAN, min<uint8_t>(telemetryRxBuffer[9], 8));
processGhostTelemetryValue(GHOST_ID_VTX_FREQ, _get_u16(frame, 2));
processGhostTelemetryValue(GHOST_ID_VTX_POWER, _get_u16(frame, 4));
processGhostTelemetryValue(GHOST_ID_VTX_CHAN, min<uint8_t>(frame[7], 8));
processGhostTelemetryValueString(sensor, vtxBandString);
break;
}
@@ -251,7 +246,7 @@ void processGhostTelemetryFrame()
{
GhostMenuFrame * packet;
GhostMenuData * lineData;
packet = (GhostMenuFrame * ) telemetryRxBuffer;
packet = (GhostMenuFrame * )buffer;
lineData = (GhostMenuData *) &reusableBuffer.ghostMenu.line[packet->lineIndex];
lineData->splitLine = 0;
reusableBuffer.ghostMenu.menuStatus = packet->menuStatus;
@@ -271,39 +266,42 @@ void processGhostTelemetryFrame()
case GHST_DL_PACK_STAT: {
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount);
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(buffer, length);
}
#endif
processGhostTelemetryValue(GHOST_ID_PACK_VOLTS, getTelemetryValue_u16le(3));
processGhostTelemetryValue(GHOST_ID_PACK_AMPS, getTelemetryValue_u16le(5));
processGhostTelemetryValue(GHOST_ID_PACK_MAH, getTelemetryValue_u16le(7) * 10);
processGhostTelemetryValue(GHOST_ID_PACK_VOLTS, _get_u16le(frame, 1));
processGhostTelemetryValue(GHOST_ID_PACK_AMPS, _get_u16le(frame, 3));
processGhostTelemetryValue(GHOST_ID_PACK_MAH, _get_u16le(frame, 5) * 10);
break;
}
case GHST_DL_GPS_PRIMARY: {
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount);
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(buffer, length);
}
#endif
processGhostTelemetryValue(GHOST_ID_GPS_LAT, ((int32_t)getTelemetryValue_s32le(3)) / 10);
processGhostTelemetryValue(GHOST_ID_GPS_LONG, ((int32_t)getTelemetryValue_s32le(7)) / 10);
processGhostTelemetryValue(GHOST_ID_GPS_ALT, (int16_t)getTelemetryValue_u16le(11));
processGhostTelemetryValue(GHOST_ID_GPS_LAT, ((int32_t)_get_s32le(frame, 1)) / 10);
processGhostTelemetryValue(GHOST_ID_GPS_LONG, ((int32_t)_get_s32le(frame, 5)) / 10);
processGhostTelemetryValue(GHOST_ID_GPS_ALT, (int16_t)_get_u16le(frame, 9));
break;
}
case GHST_DL_GPS_SECONDARY: {
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount);
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(buffer, length);
}
#endif
processGhostTelemetryValue(GHOST_ID_GPS_HDG, getTelemetryValue_u16le(5) / 10);
processGhostTelemetryValue(GHOST_ID_GPS_HDG, _get_u16le(frame, 3) / 10);
// ground speed is passed via GHST as cm/s, converted to km/h for OpenTx
processGhostTelemetryValue(GHOST_ID_GPS_GSPD, (getTelemetryValue_u16le(3) * 36 + 50) / 100);
processGhostTelemetryValue(GHOST_ID_GPS_SATS, telemetryRxBuffer[7]);
processGhostTelemetryValue(GHOST_ID_GPS_GSPD, (_get_u16le(frame, 1) * 36 + 50) / 100);
processGhostTelemetryValue(GHOST_ID_GPS_SATS, frame[5]);
break;
}
case GHST_DL_MAGBARO: {
@@ -312,10 +310,10 @@ void processGhostTelemetryFrame()
}
#if defined(LUA)
default:
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) {
for (uint8_t i=1; i<telemetryRxBufferCount-1; i++) {
// destination address and CRC are skipped
luaInputTelemetryFifo->push(telemetryRxBuffer[i]);
// destination address and CRC are skipped
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(length - 2) ) {
for (uint8_t i = 1; i < length - 1; i++) {
luaInputTelemetryFifo->push(buffer[i]);
}
}
break;
@@ -323,31 +321,6 @@ void processGhostTelemetryFrame()
}
}
void processGhostTelemetryData(uint8_t data)
{
if (telemetryRxBufferCount == 0 && data != GHST_ADDR_RADIO) {
TRACE("[GH] address 0x%02X error", data);
return;
}
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
}
else {
TRACE("[GH] array size %d error", telemetryRxBufferCount);
telemetryRxBufferCount = 0;
}
if (telemetryRxBufferCount > 4) {
uint8_t length = telemetryRxBuffer[1];
if (length + 2 == telemetryRxBufferCount) {
processGhostTelemetryFrame();
telemetryRxBufferCount = 0;
}
}
}
void ghostSetDefault(int index, uint8_t id, uint8_t subId)
{
TelemetrySensor &telemetrySensor = g_model.telemetrySensors[index];
+3 -4
View File
@@ -105,10 +105,6 @@ enum GhstVtxBand
GHST_VTX_BAND_COUNT
};
void processGhostTelemetryData(uint8_t data);
void ghostSetDefault(int index, uint8_t id, uint8_t subId);
uint8_t getGhostModuleAddr();
#if SPORT_MAX_BAUDRATE < 400000
// For radios which can't support telemetry at high rates, offer baud rate choices
// (modified vs. unmodified radios)
@@ -165,3 +161,6 @@ enum GhostFrames
GHST_MENU_CONTROL
};
void processGhostTelemetryFrame(uint8_t module, uint8_t* buffer, uint32_t length);
void ghostSetDefault(int index, uint8_t id, uint8_t subId);
+15 -23
View File
@@ -19,7 +19,6 @@
* GNU General Public License for more details.
*/
#include "opentx.h"
#include "aux_serial_driver.h"
#include "telemetry.h"
#include "io/multi_protolist.h"
@@ -157,14 +156,7 @@ bool isMultiModeScanning(uint8_t module)
static MultiBufferState guessProtocol(uint8_t module)
{
uint32_t moduleIdx = EXTERNAL_MODULE;
#if defined(INTERNAL_MODULE_MULTI)
if (isModuleMultimodule(INTERNAL_MODULE)) {
moduleIdx = INTERNAL_MODULE;
}
#endif
if (g_model.moduleData[moduleIdx].multi.rfProtocol == MODULE_SUBTYPE_MULTI_DSM2)
if (g_model.moduleData[module].multi.rfProtocol == MODULE_SUBTYPE_MULTI_DSM2)
return SpektrumTelemetryFallback;
else if (g_model.moduleData[module].multi.rfProtocol == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
return FlyskyTelemetryFallback;
@@ -434,14 +426,14 @@ static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
case FrSkyHubTelemetry:
if (len >= 4)
frskyDProcessPacket(data);
frskyDProcessPacket(module, data, len);
else
TRACE("[MP] Received Frsky HUB telemetry len %d < 4", len);
break;
case FrSkySportTelemetry:
if (len >= 4) {
if (sportProcessTelemetryPacket(data) && len >= 8) {
if (sportProcessTelemetryPacket(module, data, len) && len >= 8) {
uint8_t primId = data[1];
uint16_t dataId = *((uint16_t *)(data+2));
if (primId == DATA_FRAME && dataId == RSSI_ID) {
@@ -470,10 +462,13 @@ static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
#if defined(LUA)
case FrskySportPolling:
if (len >= 1 && outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT && data[0] == outputTelemetryBuffer.sport.physicalId) {
TRACE("MP Sending sport data out.");
sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
}
// TODO
// if (len >= 1 &&
// outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT &&
// data[0] == outputTelemetryBuffer.sport.physicalId) {
// TRACE("MP Sending sport data out.");
// sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
// }
break;
#endif
case SpectrumScannerPacket:
@@ -604,12 +599,9 @@ void processMultiTelemetryData(uint8_t data, uint8_t module)
uint8_t &rxBufferCount = getTelemetryRxBufferCount(module);
uint16_t &lastRxTS = getMultiTelemetryLastRxTS(module);
uint16_t nowMs = (uint16_t)RTOS_GET_MS();
if (nowMs - lastRxTS > 15)
setMultiTelemetryBufferState(module, NoProtocolDetected);
lastRxTS = nowMs;
// debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", getMultiTelemetryBufferState(module), data, rxBufferCount);
// debugPrintf("State: %d, byte received %02X, buflen: %d\r\n",
// getMultiTelemetryBufferState(module), data, rxBufferCount);
switch (getMultiTelemetryBufferState(module)) {
case NoProtocolDetected:
@@ -629,7 +621,7 @@ void processMultiTelemetryData(uint8_t data, uint8_t module)
case FrskyTelemetryFallback:
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
processFrskyTelemetryData(data);
processFrskySportTelemetryData(module, data, rxBuffer, rxBufferCount);
break;
case FrskyTelemetryFallbackFirstByte:
@@ -637,7 +629,7 @@ void processMultiTelemetryData(uint8_t data, uint8_t module)
setMultiTelemetryBufferState(module, MultiStatusOrFrskyData);
}
else {
processFrskyTelemetryData(data);
processFrskySportTelemetryData(module, data, rxBuffer, rxBufferCount);
if (data != 0x7e)
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
}
@@ -645,7 +637,7 @@ void processMultiTelemetryData(uint8_t data, uint8_t module)
break;
case FrskyTelemetryFallbackNextBytes:
processFrskyTelemetryData(data);
processFrskySportTelemetryData(module, data, rxBuffer, rxBufferCount);
if (data == 0x7e) {
// end of packet or start of new packet
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
+5 -1
View File
@@ -21,6 +21,8 @@
#include "opentx.h"
#include "spektrum.h"
#include "hal/module_port.h"
#include "tasks/mixer_task.h"
/*
* Documentation of the Spektrum protocol is available under
@@ -519,7 +521,9 @@ void processDSMBindPacket(uint8_t module, const uint8_t *packet)
packet[0] & 0x3F, packet[2]);
storageDirty(EE_MODEL);
restartModule(EXTERNAL_MODULE);
moduleState[module].mode = MODULE_MODE_NORMAL;
restartModuleAsync(module, 50); // ~200ms
} else if (g_model.moduleData[module].type == MODULE_TYPE_MULTIMODULE &&
g_model.moduleData[module].multi.rfProtocol ==
+98 -155
View File
@@ -25,7 +25,7 @@
#include "pulses/flysky.h"
#include "mixer_scheduler.h"
#include "io/multi_protolist.h"
#include "aux_serial_driver.h"
#include "hal/module_port.h"
#if defined(LIBOPENUI)
#include "libopenui.h"
@@ -57,12 +57,8 @@
#include "flysky_ibus.h"
#endif
#if defined(INTMODULE_USART)
#include "intmodule_serial_driver.h"
#endif
uint8_t telemetryStreaming = 0;
uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];
uint8_t telemetryRxBufferCount = 0;
uint8_t telemetryState = TELEMETRY_INIT;
@@ -94,27 +90,30 @@ uint8_t &getTelemetryRxBufferCount(uint8_t moduleIdx)
return telemetryRxBufferCount;
}
static int (*_telemetryGetByte)(void*, uint8_t*) = nullptr;
static void* _telemetryGetByteCtx = nullptr;
// TODO: move to module port driver
//
// static int (*_telemetryGetByte)(void*, uint8_t*) = nullptr;
// static void* _telemetryGetByteCtx = nullptr;
void telemetrySetGetByte(void* ctx, int (*fct)(void*, uint8_t*))
{
_telemetryGetByte = nullptr;
_telemetryGetByteCtx = ctx;
_telemetryGetByte = fct;
}
// void telemetrySetGetByte(void* ctx, int (*fct)(void*, uint8_t*))
// {
// _telemetryGetByte = nullptr;
// _telemetryGetByteCtx = ctx;
// _telemetryGetByte = fct;
// }
static bool telemetryGetByte(uint8_t* data)
{
auto _getByte = _telemetryGetByte;
auto _ctx = _telemetryGetByteCtx;
// static bool telemetryGetByte(uint8_t* data)
// {
// auto _getByte = _telemetryGetByte;
// auto _ctx = _telemetryGetByteCtx;
if (_getByte) {
return _getByte(_ctx, data);
}
// if (_getByte) {
// return _getByte(_ctx, data);
// }
return sportGetByte(data);
}
// // return sportGetByte(data);
// return false;
// }
static void (*telemetryMirrorSendByte)(void*, uint8_t) = nullptr;
static void* telemetryMirrorSendByteCtx = nullptr;
@@ -143,11 +142,10 @@ static StaticTimer_t telemetryTimerBuffer;
static void telemetryTimerCb(TimerHandle_t xTimer)
{
(void)xTimer;
if (!s_pulses_paused) {
DEBUG_TIMER_START(debugTimerTelemetryWakeup);
telemetryWakeup();
DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
}
DEBUG_TIMER_START(debugTimerTelemetryWakeup);
telemetryWakeup();
DEBUG_TIMER_STOP(debugTimerTelemetryWakeup);
}
void telemetryStart()
@@ -175,122 +173,61 @@ void telemetryStop()
}
#endif
void processTelemetryData(uint8_t data)
{
// TODO: move these to new interface
#if defined(GHOST)
if (telemetryProtocol == PROTOCOL_TELEMETRY_GHOST) {
processGhostTelemetryData(data);
return;
}
#endif
if (telemetryProtocol == PROTOCOL_TELEMETRY_SPEKTRUM ||
telemetryProtocol == PROTOCOL_TELEMETRY_DSMP) {
processSpektrumTelemetryData(EXTERNAL_MODULE, data, telemetryRxBuffer,
telemetryRxBufferCount);
return;
}
// TODO: move these to new interface
#if defined(MULTIMODULE)
if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
processMultiTelemetryData(data, EXTERNAL_MODULE);
return;
}
#endif
#if defined(PXX1)
if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT ||
telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_D ||
telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {
processFrskyTelemetryData(data);
}
#endif
}
inline bool isBadAntennaDetected()
{
if (!isRasValueValid())
return false;
if (telemetryData.swrInternal.isFresh() && telemetryData.swrInternal.value() > FRSKY_BAD_ANTENNA_THRESHOLD)
if (telemetryData.swrInternal.isFresh() &&
telemetryData.swrInternal.value() > FRSKY_BAD_ANTENNA_THRESHOLD)
return true;
if (telemetryData.swrExternal.isFresh() && telemetryData.swrExternal.value() > FRSKY_BAD_ANTENNA_THRESHOLD)
if (telemetryData.swrExternal.isFresh() &&
telemetryData.swrExternal.value() > FRSKY_BAD_ANTENNA_THRESHOLD)
return true;
return false;
}
static inline bool pollTelemetry(uint8_t module, const etx_module_driver_t* drv, void* ctx)
static inline void pollTelemetry(uint8_t module, const etx_proto_driver_t* drv, void* ctx)
{
if (!drv || !drv->getByte || !drv->processData) return false;
if (!drv || !drv->processData) return;
auto mod_st = (etx_module_state_t*)ctx;
auto serial_drv = modulePortGetSerialDrv(mod_st->rx);
auto serial_ctx = modulePortGetCtx(mod_st->rx);
if (!serial_drv || !serial_ctx || !serial_drv->getByte)
return;
uint8_t* rxBuffer = getTelemetryRxBuffer(module);
uint8_t& rxBufferCount = getTelemetryRxBufferCount(module);
uint8_t data;
if (drv->getByte(ctx, &data) > 0) {
if (serial_drv->getByte(serial_ctx, &data) > 0) {
LOG_TELEMETRY_WRITE_START();
do {
telemetryMirrorSend(data);
drv->processData(ctx, data, rxBuffer, &rxBufferCount);
LOG_TELEMETRY_WRITE_BYTE(data);
} while (drv->getByte(ctx, &data) > 0);
} while (serial_drv->getByte(serial_ctx, &data) > 0);
}
return true;
}
static inline void pollExtTelemetryLegacy()
{
uint8_t data;
if (telemetryGetByte(&data)) {
LOG_TELEMETRY_WRITE_START();
do {
telemetryMirrorSend(data);
processTelemetryData(data);
LOG_TELEMETRY_WRITE_BYTE(data);
} while (telemetryGetByte(&data));
}
}
// This can only be changed when the mixer is not
// running as the priority of the timer task is
// lower.
volatile uint8_t _telemetryIsPolling = false;
// TODO: this needs to be rewritten completely
// - telemetry polling needs to happen for each enabled module
void telemetryWakeup()
{
#if defined(HARDWARE_INTERNAL_MODULE)
auto int_drv = getIntModuleDriver();
if (int_drv) pollTelemetry(INTERNAL_MODULE, int_drv, getIntModuleCtx());
#endif
bool disable_legacy_polling = false;
#if defined(HARDWARE_EXTERNAL_MODULE)
auto ext_drv = getExtModuleDriver();
if (ext_drv) {
if (pollTelemetry(EXTERNAL_MODULE, ext_drv, getExtModuleCtx())) {
// skip legacy telemetry polling in case external module
// driver defines telemetry methods (getByte & processData)
disable_legacy_polling = true;
}
}
#endif
if (!disable_legacy_polling) {
// TODO: needs to be moved to protocol/module init
// as-is, it implies only ONE telemetry protocol
// enabled at the same time
uint8_t requiredTelemetryProtocol = modelTelemetryProtocol();
if (telemetryProtocol != requiredTelemetryProtocol) {
telemetryInit(requiredTelemetryProtocol);
}
// Poll external / S.PORT telemetry
// TODO: how to switch this OFF ???
pollExtTelemetryLegacy();
_telemetryIsPolling = true;
for (uint8_t i = 0; i < MAX_MODULES; i++) {
auto mod = pulsesGetModuleDriver(i);
if (!mod) continue;
pollTelemetry(i, mod->drv, mod->ctx);
}
_telemetryIsPolling = false;
for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
@@ -352,14 +289,20 @@ void telemetryWakeup()
if (TELEMETRY_STREAMING()) {
if (telemetryState == TELEMETRY_KO) {
AUDIO_TELEMETRY_BACK();
#if defined(CROSSFIRE)
// TODO: move to crossfire code
#if defined(HARDWARE_EXTERNAL_MODULE)
if (isModuleCrossfire(EXTERNAL_MODULE)) {
moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID;
}
#endif
#if defined(HARDWARE_INTERNAL_MODULE)
if (isModuleCrossfire(INTERNAL_MODULE)) {
moduleState[INTERNAL_MODULE].counter = CRSF_FRAME_MODELID;
}
#endif
#endif
}
telemetryState = TELEMETRY_OK;
@@ -419,49 +362,49 @@ void telemetryInit(uint8_t protocol)
{
telemetryProtocol = protocol;
if (protocol == PROTOCOL_TELEMETRY_FRSKY_D) {
telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
}
#if defined(MULTIMODULE)
else if (protocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
// The DIY Multi module always speaks 100000 baud regardless of the
// telemetry protocol in use
telemetryPortInit(MULTIMODULE_BAUDRATE, TELEMETRY_SERIAL_8E2);
#if defined(LUA)
outputTelemetryBuffer.reset();
#endif
telemetryPortSetDirectionInput();
} else if (protocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
// Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same
// since there is no real standard
telemetryPortInit(125000, TELEMETRY_SERIAL_DEFAULT);
}
#endif
// if (protocol == PROTOCOL_TELEMETRY_FRSKY_D) {
// telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
// }
// #if defined(MULTIMODULE)
// else if (protocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
// // The DIY Multi module always speaks 100000 baud regardless of the
// // telemetry protocol in use
// telemetryPortInit(MULTIMODULE_BAUDRATE, TELEMETRY_SERIAL_8E2);
// #if defined(LUA)
// outputTelemetryBuffer.reset();
// #endif
// telemetryPortSetDirectionInput();
// } else if (protocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
// // Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same
// // since there is no real standard
// telemetryPortInit(125000, TELEMETRY_SERIAL_DEFAULT);
// }
// #endif
#if defined(GHOST)
else if (protocol == PROTOCOL_TELEMETRY_GHOST) {
telemetryPortInit(GHOST_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
#if defined(LUA)
outputTelemetryBuffer.reset();
#endif
telemetryPortSetDirectionOutput();
}
#endif
// #if defined(GHOST)
// else if (protocol == PROTOCOL_TELEMETRY_GHOST) {
// telemetryPortInit(GHOST_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
// #if defined(LUA)
// outputTelemetryBuffer.reset();
// #endif
// telemetryPortSetDirectionOutput();
// }
// #endif
#if defined(AUX_SERIAL)
else if (protocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {
telemetryPortInit(0, TELEMETRY_SERIAL_DEFAULT);
}
#endif
else if (protocol == PROTOCOL_TELEMETRY_DSMP) {
// soft serial
telemetryPortInvertedInit(115200);
} else {
telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA);
#if defined(LUA)
outputTelemetryBuffer.reset();
#endif
}
// #if defined(AUX_SERIAL)
// else if (protocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {
// telemetryPortInit(0, TELEMETRY_SERIAL_DEFAULT);
// }
// #endif
// else if (protocol == PROTOCOL_TELEMETRY_DSMP) {
// // soft serial
// telemetryPortInvertedInit(115200);
// } else {
// telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA);
// #if defined(LUA)
// outputTelemetryBuffer.reset();
// #endif
// }
}
#if defined(LOG_TELEMETRY) && !defined(SIMU)
+10 -31
View File
@@ -76,9 +76,6 @@ void telemetryMirrorSend(uint8_t data);
void telemetryWakeup();
void telemetryReset();
extern uint8_t telemetryProtocol;
void telemetryInit(uint8_t protocol);
void telemetryInterrupt10ms();
void telemetryStart();
@@ -129,31 +126,9 @@ void frskyDSetDefault(int index, uint16_t id);
#define IS_DISTANCE_UNIT(unit) ((unit) == UNIT_METERS || (unit) == UNIT_FEET)
#define IS_SPEED_UNIT(unit) ((unit) >= UNIT_KTS && (unit) <= UNIT_MPH)
extern uint8_t telemetryProtocol;
inline const char* getRssiLabel()
{
#if defined(MULTIMODULE)
if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE &&
(g_model.moduleData[EXTERNAL_MODULE].multi.rfProtocol ==
MODULE_SUBTYPE_MULTI_FS_AFHDS2A ||
g_model.moduleData[EXTERNAL_MODULE].multi.rfProtocol ==
MODULE_SUBTYPE_MULTI_HOTT)) {
return "RQly";
}
#endif
#if defined(GHOST)
if (telemetryProtocol == PROTOCOL_TELEMETRY_GHOST) {
return "RQly";
}
#endif
#if defined (PCBNV14)
extern uint32_t NV14internalModuleFwVersion;
if ( (telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_NV14)
&& (NV14internalModuleFwVersion >= 0x1000E) )
return "Sgnl";
#endif
return "RSSI";
return "Rx-Stats";
}
// TODO: this should handle only the external S.PORT line
@@ -161,6 +136,7 @@ inline const char* getRssiLabel()
//
inline uint8_t modelTelemetryProtocol()
{
#if defined(HARDWARE_EXTERNAL_MODULE)
bool sportUsed = isSportLineUsedByInternalModule();
#if defined(CROSSFIRE)
@@ -175,6 +151,7 @@ inline uint8_t modelTelemetryProtocol()
}
#endif
// TODO: PPM driver should support setting up a telemetry parser callback
if (!sportUsed && isModulePPM(EXTERNAL_MODULE)) {
return g_model.telemetryProtocol;
}
@@ -191,17 +168,19 @@ inline uint8_t modelTelemetryProtocol()
}
#endif
if (isModuleDSMP(EXTERNAL_MODULE)) {
return PROTOCOL_TELEMETRY_DSMP;
}
#endif // HARDWARE_EXTERNAL_MODULE
// TODO: Check if that is really necessary...
#if defined(AFHDS2)
#if defined(AFHDS2) && defined(HARDWARE_INTERNAL_MODULE)
if (isModuleAFHDS2A(INTERNAL_MODULE)) {
return PROTOCOL_TELEMETRY_FLYSKY_NV14;
}
#endif
if (isModuleDSMP(EXTERNAL_MODULE)) {
return PROTOCOL_TELEMETRY_DSMP;
}
// default choice
return PROTOCOL_TELEMETRY_FRSKY_SPORT;
}
+12 -40
View File
@@ -57,45 +57,11 @@ uint8_t allowNewSensors;
bool isFaiForbidden(source_t idx)
{
if (idx < MIXSRC_FIRST_TELEM) {
return false;
}
if (idx < MIXSRC_FIRST_TELEM) return false;
TelemetrySensor * sensor = &g_model.telemetrySensors[(idx-MIXSRC_FIRST_TELEM)/3];
auto unit = g_model.telemetrySensors[(idx - MIXSRC_FIRST_TELEM) / 3].unit;
if (unit == UNIT_VOLTS || unit == UNIT_DB) return false;
switch (telemetryProtocol) {
case PROTOCOL_TELEMETRY_FRSKY_SPORT:
if (sensor->id == RSSI_ID) {
return false;
}
else if (sensor->id == BATT_ID) {
return false;
}
break;
case PROTOCOL_TELEMETRY_FRSKY_D:
if (sensor->id == D_RSSI_ID) {
return false;
}
else if (sensor->id == D_A1_ID) {
return false;
}
break;
#if defined(CROSSFIRE)
case PROTOCOL_TELEMETRY_CROSSFIRE:
if (sensor->id == RX_RSSI1_INDEX) {
return false;
}
else if (sensor->id == RX_RSSI2_INDEX) {
return false;
}
else if (sensor->id == BATT_VOLTAGE_INDEX) {
return false;
}
break;
#endif
}
return true;
}
@@ -105,7 +71,10 @@ uint32_t getDistFromEarthAxis(int32_t latitude)
uint32_t lat = abs(latitude) / 10000;
uint32_t angle2 = (lat * lat) / 10000;
uint32_t angle4 = angle2 * angle2;
return 139*(((uint32_t)10000000 - ((angle2*(uint32_t)123370)/81) + (angle4/25))/12500);
return 139 * (((uint32_t)10000000 - ((angle2 * (uint32_t)123370) / 81) +
(angle4 / 25)) /
12500);
}
void TelemetryItem::setValue(const TelemetrySensor & sensor, const char * val, uint32_t, uint32_t)
@@ -114,7 +83,8 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, const char * val, u
setFresh();
}
void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32_t unit, uint32_t prec)
void TelemetryItem::setValue(const TelemetrySensor &sensor, int32_t val,
uint32_t unit, uint32_t prec)
{
int32_t newVal = val;
@@ -522,7 +492,9 @@ int lastUsedTelemetryIndex()
}
template <class T>
int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, T value, uint32_t unit = 0, uint32_t prec = 0)
int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId,
uint8_t instance, T value, uint32_t unit = 0,
uint32_t prec = 0)
{
bool sensorFound = false;