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:
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -22,6 +22,7 @@
|
||||
#include "opentx.h"
|
||||
#include "flysky_nv14.h"
|
||||
#include "flysky_ibus.h"
|
||||
#include "pulses/afhds2.h"
|
||||
#include "dataconstants.h"
|
||||
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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 ==
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user