From 022bfbe13baa54717f2ad52fe5cf9fe66b093af1 Mon Sep 17 00:00:00 2001 From: Ben Date: Wed, 14 May 2025 09:59:59 +0300 Subject: [PATCH 1/5] GCS_MAVLink: add MAVn_OPTIONS parameters --- libraries/GCS_MAVLink/GCS.h | 8 ++++++++ libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp | 12 ++++++++++++ 2 files changed, 20 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index efcc3398e3bfd8..8c5b945b2c5006 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -516,6 +516,14 @@ class GCS_MAVLINK virtual bool persist_streamrates() const { return false; } void handle_request_data_stream(const mavlink_message_t &msg); + AP_Int16 options; + enum class Option : uint16_t { + // first bit is reserved for: MAVLINK2_SIGNING_DISABLED = (1U << 0), + }; + bool option_enabled(Option option) const { + return options & static_cast(option); + } + virtual void handle_command_ack(const mavlink_message_t &msg); void handle_set_mode(const mavlink_message_t &msg); void handle_command_int(const mavlink_message_t &msg); diff --git a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp index 658f936a816e3d..bf2656d29e3f48 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp @@ -202,6 +202,18 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @RebootRequired: True // @User: Advanced AP_GROUPINFO("_ADSB", 10, GCS_MAVLINK, streamRates[GCS_MAVLINK::STREAM_ADSB], DRATE(GCS_MAVLINK::STREAM_ADSB)), + + // ------------ + // IMPORTANT: Add new stream rates *before* the _OPTIONS parameter. + // ------------ + + // @Param: _OPTIONS + // @DisplayName: Bitmask for configuring this telemetry channel + // @Description: Bitmask for configuring this telemetry channel. For having effect on all channels, set the relevant mask in all MAVx_OPTIONS parameters. Keep in mind that part of the flags may require a reboot to take action. + // @RebootRequired: True + // @User: Standard + // @Bitmask: 0:Accept unsigned MAVLink2 messages + AP_GROUPINFO("_OPTIONS", 20, GCS_MAVLINK, options, 0), AP_GROUPEND }; #undef DRATE From af003a049e15f17453ab00e76af79f884690a794 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 18 May 2025 13:37:20 +1000 Subject: [PATCH 2/5] AP_HAL: move option bits from UARTDriver to MAVx we recently got options on a per-mavlink-connection basis. Use them --- libraries/AP_HAL/UARTDriver.h | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index 6428080e587160..91134d4828861f 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -86,7 +86,7 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { virtual bool set_options(uint16_t options) { _last_options = options; return options==0; } virtual uint16_t get_options(void) const { return _last_options; } - enum { + enum Option { OPTION_RXINV = (1U<<0), // invert RX line OPTION_TXINV = (1U<<1), // invert TX line OPTION_HDPLEX = (1U<<2), // half-duplex (one-wire) mode @@ -97,11 +97,16 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { OPTION_PULLUP_TX = (1U<<7), // apply pullup to TX OPTION_NODMA_RX = (1U<<8), // don't use DMA for RX OPTION_NODMA_TX = (1U<<9), // don't use DMA for TX - OPTION_MAVLINK_NO_FORWARD = (1U<<10), // don't forward MAVLink data to or from this device + OPTION_MAVLINK_NO_FORWARD_old = (1U<<10), // // moved to GCS_MAVLINK::Option OPTION_NOFIFO = (1U<<11), // disable hardware FIFO - OPTION_NOSTREAMOVERRIDE = (1U<<12), // don't allow GCS to override streamrates + OPTION_NOSTREAMOVERRIDE_old = (1U<<12), // moved to GCS_MAVLINK::Option }; + bool option_is_set(Option option) const { + return (_last_options & (uint16_t)option) != 0; + } + + enum flow_control { FLOW_CONTROL_DISABLE=0, FLOW_CONTROL_ENABLE=1, From c9f4bd5674818eeae649027f8e217b0b2d123a41 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 18 May 2025 13:37:21 +1000 Subject: [PATCH 3/5] GCS_MAVLink: move option bits from UARTDriver to MAVx we recently got options on a per-mavlink-connection basis. Use them --- libraries/GCS_MAVLink/GCS.h | 9 ++++++ libraries/GCS_MAVLink/GCS_Common.cpp | 29 +++++++++++++++---- .../GCS_MAVLink/GCS_MAVLink_Parameters.cpp | 8 ++++- 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8c5b945b2c5006..ca7d4c3f1cc99f 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -519,10 +519,19 @@ class GCS_MAVLINK AP_Int16 options; enum class Option : uint16_t { // first bit is reserved for: MAVLINK2_SIGNING_DISABLED = (1U << 0), + NO_FORWARD = (1U << 1), // don't forward MAVLink data to or from this device + NOSTREAMOVERRIDE = (1U << 2), // ignore REQUEST_DATA_STREAM messages (eg. from GCSs) }; bool option_enabled(Option option) const { return options & static_cast(option); } + void enable_option(Option option) { + options.set_and_save(static_cast(options) | static_cast(option)); + } + void disable_option(Option option) { + options.set_and_save(static_cast(options) & (~ static_cast(option))); + } + AP_Int8 options_were_converted; virtual void handle_command_ack(const mavlink_message_t &msg); void handle_set_mode(const mavlink_message_t &msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 8f3719a8208675..45fc776dcc8fda 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -152,11 +152,26 @@ bool GCS_MAVLINK::init(uint8_t instance) return false; } + // PARAMETER_CONVERSION - Added: May-2025 for ArduPilot-4.7 + // convert parameters; we used to use bits in the UARTDriver to + // remember whether the mavlink connection on that interface was + // "private" or not, and whether to ignore streamrate sets via + // REQUEST_DATA_STREAM. We moved that into the MAVn_OPTIONS, this + // is the conversion: + if (!options_were_converted) { + options_were_converted.set_and_save(1); + if (_port->option_is_set(AP_HAL::UARTDriver::Option::OPTION_MAVLINK_NO_FORWARD_old)) { + enable_option(Option::NO_FORWARD); + } + if (_port->option_is_set(AP_HAL::UARTDriver::Option::OPTION_NOSTREAMOVERRIDE_old)) { + enable_option(Option::NOSTREAMOVERRIDE); + } + } + // and init the gcs instance - // whether this port is considered "private" is stored on the uart - // rather than in our own parameters: - if (uartstate->option_enabled(AP_HAL::UARTDriver::OPTION_MAVLINK_NO_FORWARD)) { + // whether this port is considered "private": + if (option_enabled(Option::NO_FORWARD)) { set_channel_private(chan); } @@ -4417,10 +4432,12 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) #endif case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - // only pass if override is not selected - if (!(_port->get_options() & _port->OPTION_NOSTREAMOVERRIDE)) { - handle_request_data_stream(msg); + if (option_enabled(Option::NOSTREAMOVERRIDE)) { + // options indicate we are to ignore this stream rate + // request + break; } + handle_request_data_stream(msg); break; case MAVLINK_MSG_ID_DATA96: diff --git a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp index bf2656d29e3f48..dd259cb90b5340 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp @@ -212,8 +212,14 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Description: Bitmask for configuring this telemetry channel. For having effect on all channels, set the relevant mask in all MAVx_OPTIONS parameters. Keep in mind that part of the flags may require a reboot to take action. // @RebootRequired: True // @User: Standard - // @Bitmask: 0:Accept unsigned MAVLink2 messages + // @Bitmask: 1:Don't forward mavlink to/from, 2:Ignore Streamrate AP_GROUPINFO("_OPTIONS", 20, GCS_MAVLINK, options, 0), + + // PARAMETER_CONVERSION - Added: May-2025 for ArduPilot-4.7 + // Hidden param used as a flag for param conversion + // This allows one time conversion while allowing user to flash between versions with and without converted params + AP_GROUPINFO_FLAGS("_OPTIONSCNV", 21, GCS_MAVLINK, options_were_converted, 0, AP_PARAM_FLAG_HIDDEN), + AP_GROUPEND }; #undef DRATE From 3058b11a0387e2b81954fbe1c9bcbccbcc75b3bc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 18 May 2025 13:37:21 +1000 Subject: [PATCH 4/5] Tools: move option bits from UARTDriver to MAVx we recently got options on a per-mavlink-connection basis. Use them --- Tools/autotest/rover.py | 2 +- Tools/autotest/test_param_upgrade.py | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 20ab334a732ba5..43d7f5bcf20fe9 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6200,7 +6200,7 @@ def printmessage(mav, m): # ensure setting the private channel mask doesn't cause us to # execute these commands: - self.set_parameter("SERIAL2_OPTIONS", 1024) + self.set_parameter("MAV3_OPTIONS", 2) self.reboot_sitl() # mavlink-private is reboot-required mav2 = mavutil.mavlink_connection("tcp:localhost:5763", robust_parsing=True, diff --git a/Tools/autotest/test_param_upgrade.py b/Tools/autotest/test_param_upgrade.py index 5e353a70bc2bc8..39f9c22b349991 100755 --- a/Tools/autotest/test_param_upgrade.py +++ b/Tools/autotest/test_param_upgrade.py @@ -5,6 +5,9 @@ ./Tools/autotest/test_param_upgrade.py --vehicle=arduplane --param "GPS_TYPE=17->GPS1_TYPE=17" --param "GPS_TYPE2=37->GPS2_TYPE=37" --param "GPS_GNSS_MODE=21->GPS1_GNSS_MODE=21" --param "GPS_GNSS_MODE2=45->GPS2_GNSS_MODE=45" --param "GPS_RATE_MS=186->GPS1_RATE_MS=186" --param "GPS_RATE_MS2=123->GPS2_RATE_MS=123" --param "GPS_POS1_X=3.75->GPS1_POS_X=3.75" --param "GPS_POS2_X=6.9->GPS2_POS_X=6.9" --param "GPS_POS1_Y=2.75->GPS1_POS_Y=2.75" --param "GPS_POS2_Y=5.9->GPS2_POS_Y=5.9" --param "GPS_POS1_Z=12.1->GPS1_POS_Z=12.1" --param "GPS_POS2_Z=-6.9->GPS2_POS_Z=-6.9" --param "GPS_DELAY_MS=987->GPS1_DELAY_MS=987" --param "GPS_DELAY_MS2=2345->GPS2_DELAY_MS=2345" --param "GPS_COM_PORT=19->GPS1_COM_PORT=19" --param "GPS_COM_PORT2=100->GPS2_COM_PORT=100" --param "GPS_CAN_NODEID1=109->GPS1_CAN_NODEID=109" --param "GPS_CAN_NODEID2=102->GPS2_CAN_NODEID=102" --param "GPS1_CAN_OVRIDE=34->GPS1_CAN_OVRIDE=34" --param "GPS2_CAN_OVRIDE=67" --param "GPS_MB1_TYPE=1->GPS1_MB_TYPE=1" --param "GPS_MB1_OFS_X=3.14->GPS1_MB_OFS_X=3.14" --param "GPS_MB1_OFS_Y=2.18->GPS1_MB_OFS_Y=2.18" --param "GPS_MB1_OFS_Z=17.6->GPS1_MB_OFS_Z=17.6" --param "GPS_MB2_TYPE=13->GPS2_MB_TYPE=13" --param "GPS_MB2_OFS_X=17.14->GPS2_MB_OFS_X=17.14" --param "GPS_MB2_OFS_Y=12.18->GPS2_MB_OFS_Y=12.18" --param "GPS_MB2_OFS_Z=27.6->GPS2_MB_OFS_Z=27.6" # noqa +./Tools/autotest/test_param_upgrade.py --vehicle=arduplane --param "SERIAL1_OPTIONS=1024->MAV2_OPTIONS=2" --param "SERIAL2_OPTIONS=4096->MAV3_OPTIONS=4" +./Tools/autotest/test_param_upgrade.py --vehicle=arduplane --param "SERIAL1_OPTIONS=5120->MAV2_OPTIONS=6" + AP_FLAKE8_CLEAN ''' From a05d0f9843e0cce292c8838e35a3e7e01136a02b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 May 2025 17:52:48 +1000 Subject: [PATCH 5/5] AP_SerialMananger: adjust metadata for moved options bits --- libraries/AP_SerialManager/AP_SerialManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 358fe717e353c7..1811a54aad7d5c 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -272,8 +272,8 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { #if HAL_HAVE_SERIAL1_PARAMS // @Param: 1_OPTIONS // @DisplayName: Telem1 options - // @Description: Control over UART options. The InvertRX option controls invert of the receive pin. The InvertTX option controls invert of the transmit pin. The HalfDuplex option controls half-duplex (onewire) mode, where both transmit and receive is done on the transmit wire. The Swap option allows the RX and TX pins to be swapped on STM32F7 based boards. - // @Bitmask: 0:InvertRX, 1:InvertTX, 2:HalfDuplex, 3:SwapTXRX, 4: RX_PullDown, 5: RX_PullUp, 6: TX_PullDown, 7: TX_PullUp, 8: RX_NoDMA, 9: TX_NoDMA, 10: Don't forward mavlink to/from, 11: DisableFIFO, 12: Ignore Streamrate + // @Description: Control over UART options. The InvertRX option controls invert of the receive pin. The InvertTX option controls invert of the transmit pin. The HalfDuplex option controls half-duplex (onewire) mode, where both transmit and receive is done on the transmit wire. The Swap option allows the RX and TX pins to be swapped on STM32F7 based boards. NOTE that two bits have moved from this parameter into MAVn_OPTIONS! + // @Bitmask: 0:InvertRX, 1:InvertTX, 2:HalfDuplex, 3:SwapTXRX, 4: RX_PullDown, 5: RX_PullUp, 6: TX_PullDown, 7: TX_PullUp, 8: RX_NoDMA, 9: TX_NoDMA, 10: UNUSED OPTION (was Don't forward mavlink to/from); see MAVn_OPTIONS, 11: DisableFIFO, 12: UNUSED OTION (was Ignore Streamrate); see MAVn_OPTIONS // @User: Advanced // @RebootRequired: True AP_GROUPINFO("1_OPTIONS", 14, AP_SerialManager, state[1].options, DEFAULT_SERIAL1_OPTIONS),