From e6ddb2440e8c7f8d821408ee2b3a57a83eb0d2b5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 17:51:52 +0200 Subject: [PATCH 01/11] Add setting for minimum ammount of tx buffer --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 6 ++++++ src/main/telemetry/mavlink.c | 2 +- src/main/telemetry/telemetry.c | 3 ++- src/main/telemetry/telemetry.h | 1 + 5 files changed, 20 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index da0b77f8ce1..10a4e1bfd7f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2542,6 +2542,16 @@ Rate of the extra3 message for MAVLink telemetry --- +### mavlink_min_txbuffer + +Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. + +| Default | Min | Max | +| --- | --- | --- | +| | 0 | 100 | + +--- + ### mavlink_pos_rate Rate of the position message for MAVLink telemetry diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 52d6d63f66e..2f19a8d0a49 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3191,6 +3191,12 @@ groups: min: 1 max: 2 default_value: 2 + - name: mavlink_min_txbuffer + file: mavlink.min_txbuff + description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." + default: 100 + min: 0 + max: 100 - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 59d1ef69463..4becf6375f4 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1217,7 +1217,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) incomingRequestServed = true; } - if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= 90) { + if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= telemetryConfig()->mavlink.min_txbuff) { // Only process scheduled data if we didn't serve any incoming request this cycle if (!incomingRequestServed || ( diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d6c8e355d86..d2334de891a 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -89,7 +89,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_VERSION_DEFAULT + .version = SETTING_MAVLINK_VERSION_DEFAULT, + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFF_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index c7edad973b8..d74c32d77e7 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -72,6 +72,7 @@ typedef struct telemetryConfig_s { uint8_t extra2_rate; uint8_t extra3_rate; uint8_t version; + uint8_t min_txbuff; } mavlink; } telemetryConfig_t; From 6e549d69a418c133d1382e6f788c9a661b22b558 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 18:14:54 +0200 Subject: [PATCH 02/11] Update receiver quality statistics, if using mavlink as serialrx_provider --- src/main/telemetry/mavlink.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 4becf6375f4..67c4cf59b9a 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1116,6 +1116,13 @@ static bool handleIncoming_RADIO_STATUS(void) { mavlink_radio_status_t msg; mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); txbuff_free = msg.txbuf; + + if (rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + rxLinkStatistics.uplinkRSSI = -msg.remrssi; // dbM + rxLinkStatistics.uplinkSNR = msg.noise; // dbM * 2? + rxLinkStatistics.uplinkLQ = scaleRange(msg.rssi, 0, 254, 0, 100); // May be elrs specific + } return true; } From 309412df22bae620ad7f7c645585b6566d443ae0 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 18:22:36 +0200 Subject: [PATCH 03/11] Small fixes --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/telemetry/telemetry.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 10a4e1bfd7f..889cb55da17 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2548,7 +2548,7 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry | Default | Min | Max | | --- | --- | --- | -| | 0 | 100 | +| 100 | 0 | 100 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2f19a8d0a49..4ea5e73d2f2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3192,9 +3192,9 @@ groups: max: 2 default_value: 2 - name: mavlink_min_txbuffer - file: mavlink.min_txbuff + field: mavlink.min_txbuff description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." - default: 100 + default_value: 100 min: 0 max: 100 diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d2334de891a..4200c18c6a0 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -90,7 +90,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, .version = SETTING_MAVLINK_VERSION_DEFAULT, - .min_txbuff = SETTING_MAVLINK_MIN_TXBUFF_DEFAULT + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT } ); From 3125fc08fdbcac60b5779b5acff372f21bf72501 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 18:24:18 +0200 Subject: [PATCH 04/11] Bump pg version --- src/main/telemetry/telemetry.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 4200c18c6a0..0d846ad19a8 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -53,7 +53,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 7); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, From 44d99f3dd415064298a2a009f22d60e7e9193d65 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 11 Jul 2024 15:48:39 +0200 Subject: [PATCH 05/11] improve half duplex checks --- src/main/telemetry/mavlink.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 67c4cf59b9a..39ac3524d29 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1207,6 +1207,11 @@ static bool processMAVLinkIncomingTelemetry(void) return false; } +static bool isMAVLinkTelemetryHalfDuplex(void) { + return (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex)) + || telemetryConfig()->halfDuplex; +} + void handleMAVLinkTelemetry(timeUs_t currentTimeUs) { static bool incomingRequestServed; @@ -1226,16 +1231,10 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= telemetryConfig()->mavlink.min_txbuff) { // Only process scheduled data if we didn't serve any incoming request this cycle - if (!incomingRequestServed || - ( - (rxConfig()->receiverType == RX_TYPE_SERIAL) && - (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) && - !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex) - ) - ) { + if (!incomingRequestServed || !isMAVLinkTelemetryHalfDuplex()) { processMAVLinkTelemetry(currentTimeUs); + lastMavlinkMessage = currentTimeUs; } - lastMavlinkMessage = currentTimeUs; incomingRequestServed = false; } } From edd2a67ce8cf77b22371e7e17922f05905eaaee9 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 16:14:31 +0200 Subject: [PATCH 06/11] Sync txbuff value with @MUSTARDTIGERFPV --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/telemetry/mavlink.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c588e31169c..3926fb1bb5f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2548,7 +2548,7 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry | Default | Min | Max | | --- | --- | --- | -| 100 | 0 | 100 | +| 33 | 0 | 100 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 35110159105..57511737f0e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3194,7 +3194,7 @@ groups: - name: mavlink_min_txbuffer field: mavlink.min_txbuff description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." - default_value: 100 + default_value: 33 min: 0 max: 100 diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ee372a3cfc1..1c58f36e493 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1253,7 +1253,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) // Determine whether to send telemetry back based on flow control / pacing if (txbuff_valid) { // Use flow control if available - shouldSendTelemetry = txbuff_free >= 33; + shouldSendTelemetry = txbuff_free >= telmetryConfig()->mavlink.min_txbuff; } else { // If not, use blind frame pacing - and back off for collision avoidance if half-duplex bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); From ca22dba0ca581bd4514404308876713751fd63c9 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 16:40:21 +0200 Subject: [PATCH 07/11] Fix typo --- src/main/telemetry/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 1c58f36e493..0e30dc00ac6 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1253,7 +1253,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) // Determine whether to send telemetry back based on flow control / pacing if (txbuff_valid) { // Use flow control if available - shouldSendTelemetry = txbuff_free >= telmetryConfig()->mavlink.min_txbuff; + shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff; } else { // If not, use blind frame pacing - and back off for collision avoidance if half-duplex bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); From 7a420cbdc20c2329cd65cf99f217f57139d42be1 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:03:07 +0200 Subject: [PATCH 08/11] Parse rssi/lq infor from radio_status --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 9 +++++++++ src/main/telemetry/mavlink.c | 26 ++++++++++++++++++++++---- src/main/telemetry/telemetry.c | 3 ++- src/main/telemetry/telemetry.h | 7 +++++++ 5 files changed, 50 insertions(+), 5 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3926fb1bb5f..9f34dd5f479 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2562,6 +2562,16 @@ Rate of the position message for MAVLink telemetry --- +### mavlink_radio_type + +Mavlink radio type. Affects how RSSI and LQ are reported on OSD. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + ### mavlink_rc_chan_rate Rate of the RC channels message for MAVLink telemetry diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 57511737f0e..d1fafb27c10 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -198,6 +198,9 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e + - name: mavlink_radio_type + values: ["GENERIC", "ELRS", "SIK"] + enum: mavlinkRadio_e constants: RPYL_PID_MIN: 0 @@ -3197,6 +3200,12 @@ groups: default_value: 33 min: 0 max: 100 + - name: mavlink_radio_type + description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD." + field: mavlink.radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 0e30dc00ac6..9d9babe64f7 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1126,6 +1126,27 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { return true; } +static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { + switch(telemetryConfig()->mavlink.radio_type) { + case MAVLINK_RADIO_SIK: + rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; + rxLinkStatistics.uplinkSNR = msg->noise * 1.9; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + case MAVLINK_RADIO_ELRS: + rxLinkStatistics.uplinkRSSI = -msg->remrssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); + break; + case MAVLINK_RADIO_GENERIC: + default: + rxLinkStatistics.uplinkRSSI = msg->rssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + } +} + static bool handleIncoming_RADIO_STATUS(void) { mavlink_radio_status_t msg; mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); @@ -1134,10 +1155,7 @@ static bool handleIncoming_RADIO_STATUS(void) { if (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { - // TODO: decide what to do here - rxLinkStatistics.uplinkRSSI = -msg.remrssi; // dbM - rxLinkStatistics.uplinkSNR = msg.noise; // dbM * 2? - rxLinkStatistics.uplinkLQ = scaleRange(msg.rssi, 0, 254, 0, 100); // May be elrs specific + mavlinkParseRxStats(&msg); } return true; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 0d846ad19a8..8a8796eecb8 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -90,7 +90,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, .version = SETTING_MAVLINK_VERSION_DEFAULT, - .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index d74c32d77e7..4ace4c5943d 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -36,6 +36,12 @@ typedef enum { LTM_RATE_SLOW } ltmUpdateRate_e; +typedef enum { + MAVLINK_RADIO_GENERIC, + MAVLINK_RADIO_ELRS, + MAVLINK_RADIO_SIK, +} mavlinkRadio_e; + typedef enum { SMARTPORT_FUEL_UNIT_PERCENT, SMARTPORT_FUEL_UNIT_MAH, @@ -73,6 +79,7 @@ typedef struct telemetryConfig_s { uint8_t extra3_rate; uint8_t version; uint8_t min_txbuff; + uint8_t radio_type; } mavlink; } telemetryConfig_t; From b83ddda361d63eec884ba467dbc35bf3e5f11929 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:04:58 +0200 Subject: [PATCH 09/11] Bump PG_VERSION --- src/main/telemetry/telemetry.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 8a8796eecb8..89eca3b1dd6 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -53,7 +53,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, From 3395dadf518f688f2986209a2bf976bcbbcab9d7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:07:28 +0200 Subject: [PATCH 10/11] Fix scaling --- src/main/telemetry/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 9d9babe64f7..19bec9f62c8 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1130,7 +1130,7 @@ static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { switch(telemetryConfig()->mavlink.radio_type) { case MAVLINK_RADIO_SIK: rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; - rxLinkStatistics.uplinkSNR = msg->noise * 1.9; + rxLinkStatistics.uplinkSNR = msg->noise / 1.9; rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; break; case MAVLINK_RADIO_ELRS: From 9d69a15e4d88d7601712ba2d995308df1a8faf72 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:14:29 +0200 Subject: [PATCH 11/11] Add comment with source of dbm formula --- src/main/telemetry/mavlink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 19bec9f62c8..617c883e4f6 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1129,6 +1129,7 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { switch(telemetryConfig()->mavlink.radio_type) { case MAVLINK_RADIO_SIK: + // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; rxLinkStatistics.uplinkSNR = msg->noise / 1.9; rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;