From e4627d030d894234debd667cfc91c5879ad3b5e3 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sat, 2 Nov 2024 22:11:19 +0000 Subject: [PATCH] Add stats and info MSP commands for MSP RC This is an initial, basic implementation. This can be expanded in the future to deal with multiple sublinks etc. --- src/main/fc/fc_msp.c | 36 ++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_common.h | 37 +++++++++++++++------------ src/main/rx/rx.h | 16 +++++++----- 3 files changed, 66 insertions(+), 23 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f87b7f079c7..afa5df2741d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2914,6 +2914,42 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; +#ifdef USE_RX_MSP + case MSP2_COMMON_SET_MSP_RC_LINK_STATS: + if (dataSize == 48) { + uint8_t sublinkID = sbufReadU8(src); // Sublink ID + sbufReadU8(src); // Valid link (Failsafe backup) + if (sublinkID == 1) { + rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); + rxLinkStatistics.downlinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkSNR = sbufReadU8(src); + } + } else + return MSP_RESULT_ERROR; + break; + + case MSP2_COMMON_SET_MSP_RC_INFO: + if (dataSize == 104) { + uint8_t sublinkID = sbufReadU8(src); + + if (sublinkID == 1) { + rxLinkStatistics.uplinkTXPower = sbufReadU8(src); + rxLinkStatistics.downlinkTXPower = sbufReadU8(src); + + for (int i = 0; i < 4 - 1; i++) { + rxLinkStatistics.band[i] = sbufReadU8(src); + } + + for (int i = 0; i < 6 - 1; i++) { + rxLinkStatistics.mode[i] = sbufReadU8(src); + } + } + } else + return MSP_RESULT_ERROR; + break; +#endif + case MSP_SET_FAILSAFE_CONFIG: if (dataSize == 20) { failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index e778a1808c9..8784b253114 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -15,22 +15,25 @@ * along with INAV. If not, see . */ -#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) -#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) -#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting -#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting +#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) +#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) +#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting +#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting + +#define MSP2_COMMON_MOTOR_MIXER 0x1005 +#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 + +#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). +#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings + +#define MSP2_COMMON_SERIAL_CONFIG 0x1009 +#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A + +// radar commands +#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information +#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display -#define MSP2_COMMON_MOTOR_MIXER 0x1005 -#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 +#define MSP2_COMMON_SET_MSP_RC_LINK_STATS 0x100D //in message Sets the MSP RC stats +#define MSP2_COMMON_SET_MSP_RC_INFO 0x100E //in message Sets the MSP RC info -#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). -#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings - -#define MSP2_COMMON_SERIAL_CONFIG 0x1009 -#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A - -// radar commands -#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information -#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display - -#define MSP2_BETAFLIGHT_BIND 0x3000 +#define MSP2_BETAFLIGHT_BIND 0x3000 diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index c841838a5ea..4d8a76fecf5 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -181,12 +181,16 @@ typedef enum { } rssiSource_e; typedef struct rxLinkStatistics_s { - int16_t uplinkRSSI; // RSSI value in dBm - uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] - int8_t uplinkSNR; // The SNR of the uplink in dB - uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] - uint16_t uplinkTXPower; // power in mW - uint8_t activeAntenna; + int16_t uplinkRSSI; // RSSI value in dBm + uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] + uint8_t downlinkLQ; // A protocol specific measure of the link quality in [0..100] + int8_t uplinkSNR; // The SNR of the uplink in dB + uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] + uint16_t uplinkTXPower; // power in mW + uint16_t downlinkTXPower; // power in mW + uint8_t activeAntenna; + char band[4]; + char mode[6]; } rxLinkStatistics_t; typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data