Skip to content

Commit ef35225

Browse files
MrD-RCMrD-RC
authored
Merge pull request #10451 from iNavFlight/MrD_mLRS-MSP-message
2 parents 7f9a23d + 619948e commit ef35225

File tree

14 files changed

+232
-80
lines changed

14 files changed

+232
-80
lines changed

‎docs/Settings.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5178,7 +5178,7 @@ Value below which Crossfire SNR Alarm pops-up. (dB)
51785178

51795179
| Default | Min | Max |
51805180
| --- | --- | --- |
5181-
| 4 | -20 | 10 |
5181+
| 4 | -20 | 99 |
51825182

51835183
---
51845184

‎src/main/cms/cms_menu_osd.c‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -147,10 +147,10 @@ static const OSD_Entry menuCrsfRxEntries[]=
147147
OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM),
148148
OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM),
149149
OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN),
150-
OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM),
151-
OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ),
152-
OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB),
153-
OSD_ELEMENT_ENTRY("TX POWER", OSD_CRSF_TX_POWER),
150+
OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_RSSI_DBM),
151+
OSD_ELEMENT_ENTRY("RX LQ", OSD_LQ_UPLINK),
152+
OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_SNR_DB),
153+
OSD_ELEMENT_ENTRY("TX POWER", OSD_TX_POWER_UPLINK),
154154

155155
OSD_BACK_AND_END_ENTRY,
156156
};

‎src/main/common/streambuf.c‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,11 @@ uint8_t sbufReadU8(sbuf_t *src)
9898
return *src->ptr++;
9999
}
100100

101+
int8_t sbufReadI8(sbuf_t *src)
102+
{
103+
return *src->ptr++;
104+
}
105+
101106
uint16_t sbufReadU16(sbuf_t *src)
102107
{
103108
uint16_t ret;

‎src/main/common/streambuf.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val);
4242
void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val);
4343

4444
uint8_t sbufReadU8(sbuf_t *src);
45+
int8_t sbufReadI8(sbuf_t *src);
4546
uint16_t sbufReadU16(sbuf_t *src);
4647
uint32_t sbufReadU32(sbuf_t *src);
4748
void sbufReadData(const sbuf_t *dst, void *data, int len);

‎src/main/drivers/osd_symbols.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -234,6 +234,8 @@
234234
#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center
235235
#define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing
236236
#define SYM_ODOMETER 0x168 // 360 Odometer
237+
#define SYM_RX_BAND 0x169 // 361 RX Band
238+
#define SYM_RX_MODE 0x16A // 362 RX Mode
237239

238240
#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3
239241
#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4

‎src/main/fc/fc_msp.c‎

Lines changed: 53 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
#include "common/color.h"
3535
#include "common/maths.h"
3636
#include "common/streambuf.h"
37+
#include "common/string_light.h"
3738
#include "common/bitarray.h"
3839
#include "common/time.h"
3940
#include "common/utils.h"
@@ -215,7 +216,7 @@ static void mspSerialPassthroughFn(serialPort_t *serialPort)
215216

216217
static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
217218
{
218-
const unsigned int dataSize = sbufBytesRemaining(src);
219+
const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */
219220

220221
if (dataSize == 0) {
221222
// Legacy format
@@ -1807,7 +1808,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
18071808
#ifdef USE_FLASHFS
18081809
static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
18091810
{
1810-
const unsigned int dataSize = sbufBytesRemaining(src);
1811+
const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */
18111812
uint16_t readLength;
18121813

18131814
const uint32_t readAddress = sbufReadU32(src);
@@ -1831,7 +1832,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
18311832
uint8_t tmp_u8;
18321833
uint16_t tmp_u16;
18331834

1834-
const unsigned int dataSize = sbufBytesRemaining(src);
1835+
const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */
18351836

18361837
switch (cmdMSP) {
18371838
case MSP_SELECT_SETTING:
@@ -1861,6 +1862,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
18611862
}
18621863
rxMspFrameReceive(frame, channelCount);
18631864
}
1865+
1866+
return MSP_RESULT_NO_REPLY;
18641867
}
18651868
break;
18661869
#endif
@@ -2917,6 +2920,53 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
29172920
return MSP_RESULT_ERROR;
29182921
break;
29192922

2923+
#ifdef USE_RX_MSP
2924+
case MSP2_COMMON_SET_MSP_RC_LINK_STATS: {
2925+
if (dataSize >= 7) {
2926+
uint8_t sublinkID = sbufReadU8(src); // Sublink ID
2927+
sbufReadU8(src); // Valid link (Failsafe backup)
2928+
if (sublinkID == 0) {
2929+
setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI %
2930+
rxLinkStatistics.uplinkRSSI = -sbufReadU8(src);
2931+
rxLinkStatistics.downlinkLQ = sbufReadU8(src);
2932+
rxLinkStatistics.uplinkLQ = sbufReadU8(src);
2933+
rxLinkStatistics.uplinkSNR = sbufReadI8(src);
2934+
}
2935+
2936+
return MSP_RESULT_NO_REPLY;
2937+
} else
2938+
return MSP_RESULT_ERROR;
2939+
}
2940+
break;
2941+
2942+
case MSP2_COMMON_SET_MSP_RC_INFO: {
2943+
if (dataSize >= 15) {
2944+
uint8_t sublinkID = sbufReadU8(src);
2945+
2946+
if (sublinkID == 0) {
2947+
rxLinkStatistics.uplinkTXPower = sbufReadU16(src);
2948+
rxLinkStatistics.downlinkTXPower = sbufReadU16(src);
2949+
2950+
for (int i = 0; i < 4; i++) {
2951+
rxLinkStatistics.band[i] = sbufReadU8(src);
2952+
}
2953+
2954+
sl_toupperptr(rxLinkStatistics.band);
2955+
2956+
for (int i = 0; i < 6; i++) {
2957+
rxLinkStatistics.mode[i] = sbufReadU8(src);
2958+
}
2959+
2960+
sl_toupperptr(rxLinkStatistics.mode);
2961+
}
2962+
2963+
return MSP_RESULT_NO_REPLY;
2964+
} else
2965+
return MSP_RESULT_ERROR;
2966+
}
2967+
break;
2968+
#endif
2969+
29202970
case MSP_SET_FAILSAFE_CONFIG:
29212971
if (dataSize == 20) {
29222972
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);

‎src/main/fc/settings.yaml‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3371,35 +3371,35 @@ groups:
33713371
min: -550
33723372
max: 1250
33733373
- name: osd_snr_alarm
3374-
condition: USE_SERIALRX_CRSF
3374+
condition: USE_SERIALRX_CRSF || USE_RX_MSP
33753375
description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
33763376
default_value: 4
33773377
field: snr_alarm
33783378
min: -20
3379-
max: 10
3379+
max: 99
33803380
- name: osd_link_quality_alarm
3381-
condition: USE_SERIALRX_CRSF
3381+
condition: USE_SERIALRX_CRSF || USE_RX_MSP
33823382
description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%"
33833383
default_value: 70
33843384
field: link_quality_alarm
33853385
min: 0
33863386
max: 100
33873387
- name: osd_rssi_dbm_alarm
3388-
condition: USE_SERIALRX_CRSF
3388+
condition: USE_SERIALRX_CRSF || USE_RX_MSP
33893389
description: "RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm"
33903390
default_value: 0
33913391
field: rssi_dbm_alarm
33923392
min: -130
33933393
max: 0
33943394
- name: osd_rssi_dbm_max
3395-
condition: USE_SERIALRX_CRSF
3395+
condition: USE_SERIALRX_CRSF || USE_RX_MSP
33963396
description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%"
33973397
default_value: -30
33983398
field: rssi_dbm_max
33993399
min: -50
34003400
max: 0
34013401
- name: osd_rssi_dbm_min
3402-
condition: USE_SERIALRX_CRSF
3402+
condition: USE_SERIALRX_CRSF || USE_RX_MSP
34033403
description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%"
34043404
default_value: -120
34053405
field: rssi_dbm_min

‎src/main/io/osd.c‎

Lines changed: 79 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -224,8 +224,8 @@ static bool osdDisplayHasCanvas;
224224

225225
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
226226

227-
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13);
228-
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2);
227+
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14);
228+
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3);
229229

230230
void osdStartedSaveProcess(void) {
231231
savingSettings = true;
@@ -2467,15 +2467,15 @@ static bool osdDrawSingleElement(uint8_t item)
24672467
return true;
24682468
}
24692469

2470-
#if defined(USE_SERIALRX_CRSF)
2471-
case OSD_CRSF_RSSI_DBM:
2470+
#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
2471+
case OSD_RSSI_DBM:
24722472
{
24732473
int16_t rssi = rxLinkStatistics.uplinkRSSI;
24742474
buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna
24752475
if (rssi <= -100) {
24762476
tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM);
24772477
} else {
2478-
tfp_sprintf(buff + 1, "%3d%c%c", rssi, SYM_DBM, ' ');
2478+
tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM);
24792479
}
24802480
if (!failsafeIsReceivingRxData()){
24812481
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@@ -2484,19 +2484,15 @@ static bool osdDrawSingleElement(uint8_t item)
24842484
}
24852485
break;
24862486
}
2487-
case OSD_CRSF_LQ:
2487+
case OSD_LQ_UPLINK:
24882488
{
24892489
buff[0] = SYM_LQ;
2490-
int16_t statsLQ = rxLinkStatistics.uplinkLQ;
2491-
int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
2492-
switch (osdConfig()->crsf_lq_format) {
2493-
case OSD_CRSF_LQ_TYPE1:
2494-
if (!failsafeIsReceivingRxData()) {
2495-
tfp_sprintf(buff+1, "%3d", 0);
2496-
} else {
2497-
tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
2498-
}
2499-
break;
2490+
uint8_t lqFormat = osdConfig()->crsf_lq_format;
2491+
2492+
if (rxConfig()->receiverType == RX_TYPE_MSP)
2493+
lqFormat = OSD_CRSF_LQ_TYPE1;
2494+
2495+
switch (lqFormat) {
25002496
case OSD_CRSF_LQ_TYPE2:
25012497
if (!failsafeIsReceivingRxData()) {
25022498
tfp_sprintf(buff+1, "%s:%3d", " ", 0);
@@ -2508,9 +2504,18 @@ static bool osdDrawSingleElement(uint8_t item)
25082504
if (!failsafeIsReceivingRxData()) {
25092505
tfp_sprintf(buff+1, "%3d", 0);
25102506
} else {
2507+
int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300);
25112508
tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ);
25122509
}
25132510
break;
2511+
case OSD_CRSF_LQ_TYPE1:
2512+
default:
2513+
if (!failsafeIsReceivingRxData()) {
2514+
tfp_sprintf(buff+1, "%3d", 0);
2515+
} else {
2516+
tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
2517+
}
2518+
break;
25142519
}
25152520
if (!failsafeIsReceivingRxData()) {
25162521
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@@ -2520,7 +2525,24 @@ static bool osdDrawSingleElement(uint8_t item)
25202525
break;
25212526
}
25222527

2523-
case OSD_CRSF_SNR_DB:
2528+
case OSD_LQ_DOWNLINK:
2529+
{
2530+
buff[0] = SYM_LQ;
2531+
if (!failsafeIsReceivingRxData()) {
2532+
tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN);
2533+
} else {
2534+
tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN);
2535+
}
2536+
2537+
if (!failsafeIsReceivingRxData()) {
2538+
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
2539+
} else if (rxLinkStatistics.downlinkLQ < osdConfig()->link_quality_alarm) {
2540+
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
2541+
}
2542+
break;
2543+
}
2544+
2545+
case OSD_SNR_DB:
25242546
{
25252547
static pt1Filter_t snrFilterState;
25262548
static timeMs_t snrUpdated = 0;
@@ -2539,23 +2561,49 @@ static bool osdDrawSingleElement(uint8_t item)
25392561
}
25402562
} else if (snrFiltered <= osdConfig()->snr_alarm) {
25412563
buff[0] = SYM_SNR;
2542-
if (snrFiltered <= -10) {
2564+
if (snrFiltered <= -10 || snrFiltered >= 10) {
25432565
tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB);
25442566
} else {
2545-
tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' ');
2567+
tfp_sprintf(buff + 1, " %2d%c", snrFiltered, SYM_DB);
25462568
}
25472569
}
25482570
break;
25492571
}
25502572

2551-
case OSD_CRSF_TX_POWER:
2573+
case OSD_TX_POWER_UPLINK:
25522574
{
25532575
if (!failsafeIsReceivingRxData())
2554-
tfp_sprintf(buff, "%s%c", " ", SYM_BLANK);
2576+
tfp_sprintf(buff, "%s%c", " ", SYM_MW);
25552577
else
25562578
tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW);
25572579
break;
25582580
}
2581+
2582+
case OSD_RX_POWER_DOWNLINK:
2583+
{
2584+
if (!failsafeIsReceivingRxData())
2585+
tfp_sprintf(buff, "%s%c%c", " ", SYM_MW, SYM_AH_DECORATION_DOWN);
2586+
else
2587+
tfp_sprintf(buff, "%4d%c%c", rxLinkStatistics.downlinkTXPower, SYM_MW, SYM_AH_DECORATION_DOWN);
2588+
break;
2589+
}
2590+
case OSD_RX_BAND:
2591+
displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND);
2592+
strcat(buff, rxLinkStatistics.band);
2593+
if (strlen(rxLinkStatistics.band) < 4)
2594+
for (uint8_t i = strlen(rxLinkStatistics.band); i < 4; i++)
2595+
buff[i] = ' ';
2596+
buff[4] = '\0';
2597+
break;
2598+
2599+
case OSD_RX_MODE:
2600+
displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE);
2601+
strcat(buff, rxLinkStatistics.mode);
2602+
if (strlen(rxLinkStatistics.mode) < 6)
2603+
for (uint8_t i = strlen(rxLinkStatistics.mode); i < 6; i++)
2604+
buff[i] = ' ';
2605+
buff[6] = '\0';
2606+
break;
25592607
#endif
25602608

25612609
case OSD_FORMATION_FLIGHT:
@@ -3996,7 +4044,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
39964044
.adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT,
39974045
.adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT,
39984046
#endif
3999-
#ifdef USE_SERIALRX_CRSF
4047+
#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
40004048
.snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT,
40014049
.crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT,
40024050
.link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT,
@@ -4147,11 +4195,15 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
41474195
osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3);
41484196
osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
41494197

4150-
#ifdef USE_SERIALRX_CRSF
4151-
osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12);
4152-
osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11);
4153-
osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9);
4154-
osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10);
4198+
#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
4199+
osdLayoutsConfig->item_pos[0][OSD_RSSI_DBM] = OSD_POS(23, 12);
4200+
osdLayoutsConfig->item_pos[0][OSD_LQ_UPLINK] = OSD_POS(23, 10);
4201+
osdLayoutsConfig->item_pos[0][OSD_LQ_DOWNLINK] = OSD_POS(23, 11);
4202+
osdLayoutsConfig->item_pos[0][OSD_SNR_DB] = OSD_POS(24, 9);
4203+
osdLayoutsConfig->item_pos[0][OSD_TX_POWER_UPLINK] = OSD_POS(24, 10);
4204+
osdLayoutsConfig->item_pos[0][OSD_RX_POWER_DOWNLINK] = OSD_POS(24, 11);
4205+
osdLayoutsConfig->item_pos[0][OSD_RX_BAND] = OSD_POS(24, 12);
4206+
osdLayoutsConfig->item_pos[0][OSD_RX_MODE] = OSD_POS(24, 13);
41554207
#endif
41564208

41574209
osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);

0 commit comments

Comments
 (0)