diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index 25b1bd0f15..0ab0399c59 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -55,6 +55,7 @@ #include "io/spektrum_vtx_control.h" #include "sensors/battery.h" +#include "sensors/adcinternal.h" #include "telemetry/telemetry.h" #include "telemetry/srxl.h" @@ -118,7 +119,8 @@ typedef struct } STRU_TELE_QOS; */ -#define STRU_TELE_QOS_EMPTY_REPORT_COUNT 14 +#define STRU_TELE_QOS_EMPTY_FIELDS_COUNT 14 +#define STRU_TELE_QOS_EMPTY_FIELDS_VALUE 0xff bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs) { @@ -127,8 +129,9 @@ bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs) sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); - sbufFill(dst, 0xFF, STRU_TELE_QOS_EMPTY_REPORT_COUNT); // Clear remainder + sbufFill(dst, STRU_TELE_QOS_EMPTY_FIELDS_VALUE, STRU_TELE_QOS_EMPTY_FIELDS_COUNT); // Clear remainder + // Mandatory frame, send it unconditionally. return true; } @@ -147,21 +150,34 @@ typedef struct */ #define STRU_TELE_RPM_EMPTY_FIELDS_COUNT 8 +#define STRU_TELE_RPM_EMPTY_FIELDS_VALUE 0xff + +#define SPEKTRUM_RPM_UNUSED 0xffff +#define SPEKTRUM_TEMP_UNUSED 0x7fff bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs) { + + int16_t coreTemp = SPEKTRUM_TEMP_UNUSED; +#if defined(USE_ADC_INTERNAL) + coreTemp = getCoreTemperatureCelsius(); + coreTemp = coreTemp * 9 / 5 + 32; // C -> F +#endif + UNUSED(currentTimeUs); sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); - sbufWriteU16BigEndian(dst, 0xFFFF); // pulse leading edges + sbufWriteU16BigEndian(dst, SPEKTRUM_RPM_UNUSED); // pulse leading edges if (telemetryConfig()->report_cell_voltage) { sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V } else { sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V } - sbufWriteU16BigEndian(dst, 0x7FFF); // temperature - sbufFill(dst, 0xFF, STRU_TELE_RPM_EMPTY_FIELDS_COUNT); + sbufWriteU16BigEndian(dst, coreTemp); // temperature + sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT); + + // Mandatory frame, send it unconditionally. return true; } @@ -280,6 +296,7 @@ typedef struct } STRU_TELE_GPS_STAT; */ +#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE 0xff #define STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT 6 #define SPEKTRUM_TIME_UNKNOWN 0xFFFFFFFF @@ -327,7 +344,7 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) sbufWriteU32(dst, timeBcd); sbufWriteU8(dst, numSatBcd); sbufWriteU8(dst, altitudeHighBcd); - sbufFill(dst, 0xFF, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT); + sbufFill(dst, STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT); return true; } @@ -348,6 +365,11 @@ typedef struct UINT16 spare; // Not used } STRU_TELE_FP_MAH; */ +#define STRU_TELE_FP_EMPTY_FIELDS_COUNT 2 +#define STRU_TELE_FP_EMPTY_FIELDS_VALUE 0xff + +#define SPEKTRUM_AMPS_UNUSED 0x7fff +#define SPEKTRUM_AMPH_UNUSED 0x7fff #define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s @@ -361,17 +383,21 @@ bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs) timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh; - if ( (amps != sentAmps) || (mah != sentMah) || + + if ( amps != sentAmps || + mah != sentMah || keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) { + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH); sbufWriteU8(dst, SRXL_FRAMETYPE_SID); sbufWriteU16(dst, amps); sbufWriteU16(dst, mah); - sbufWriteU16(dst, 0x7fff); // temp A - sbufWriteU16(dst, 0x7fff); // Amps B - sbufWriteU16(dst, 0x7fff); // mAH B - sbufWriteU16(dst, 0x7fff); // temp B - sbufWriteU16(dst, 0xffff); + sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp A + sbufWriteU16(dst, SPEKTRUM_AMPS_UNUSED); // Amps B + sbufWriteU16(dst, SPEKTRUM_AMPH_UNUSED); // mAH B + sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp B + + sbufFill(dst, STRU_TELE_FP_EMPTY_FIELDS_VALUE, STRU_TELE_FP_EMPTY_FIELDS_COUNT); sentAmps = amps; sentMah = mah; @@ -553,7 +579,9 @@ typedef struct } STRU_TELE_VTX; */ -#define STRU_TELE_VTX_RESERVE_COUNT 7 +#define STRU_TELE_VTX_EMPTY_COUNT 7 +#define STRU_TELE_VTX_EMPTY_VALUE 0xff + #define VTX_KEEPALIVE_TIME_OUT 2000000 // uS static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs) @@ -579,7 +607,7 @@ static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs) sbufWriteU16(dst, vtx.powerValue); sbufWriteU8(dst, vtx.region); - sbufFill(dst, 0xFF, STRU_TELE_VTX_RESERVE_COUNT); + sbufFill(dst, STRU_TELE_VTX_EMPTY_VALUE, STRU_TELE_VTX_EMPTY_COUNT); memcpy(&vtxSent, &vtx, sizeof(spektrumVtx_t)); lastTimeSentVtx = currentTimeUs;