From d30fa62f40624b83d910b9cbeb4e48df356d3fdb Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 14 Oct 2025 00:15:04 -0800 Subject: [PATCH] mavlink: take in @dakejahl 's refactoring and extend it --- src/modules/mavlink/mavlink_main.cpp | 6 +- src/modules/mavlink/mavlink_main.h | 87 +++++++----------------- src/modules/mavlink/mavlink_receiver.cpp | 4 +- 3 files changed, 31 insertions(+), 66 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0312a0b6b2..0d5b771761 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -187,7 +187,7 @@ Mavlink::mavlink_update_parameters() { updateParams(); - set_protocol_version(_param_mav_proto_ver.get()); + setProtocolVersion(_param_mav_proto_ver.get()); if (_param_mav_type.get() < 0 || _param_mav_type.get() >= MAV_TYPE_ENUM_END) { _param_mav_type.set(0); @@ -272,7 +272,7 @@ Mavlink::set_instance_id() return false; } -void Mavlink::set_protocol_version(unsigned version) +void Mavlink::setProtocolVersion(uint8_t version) { if (version == 1) { get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; @@ -3041,7 +3041,7 @@ Mavlink::display_status() } printf("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off"); - printf("\tMAVLink version: %" PRId32 "\n", _protocol_version); + printf("\tMAVLink version: %" PRId8 "\n", _protocol_version); printf("\ttransport protocol: "); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 18058bd54b..cdf079f9db 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -109,22 +109,10 @@ class Mavlink final : public ModuleParams { public: - /** - * Constructor - */ Mavlink(); - - /** - * Destructor, also kills the mavlinks task. - */ ~Mavlink(); - /** - * Start the mavlink task. - * - * @return OK on success. - */ - static int start(int argc, char *argv[]); + static int start(int argc, char *argv[]); bool running() const { return _task_running.load(); } bool should_exit() const { return _task_should_exit.load(); } @@ -134,63 +122,40 @@ public: _receiver.request_stop(); } - /** - * Display the mavlink status. - */ - void display_status(); + void display_status(); + void display_status_streams(); - /** - * Display the status of all enabled streams. - */ - void display_status_streams(); + static int stop_command(int argc, char *argv[]); + static int stream_command(int argc, char *argv[]); - static int stop_command(int argc, char *argv[]); - static int stream_command(int argc, char *argv[]); + static int instance_count(); + static Mavlink *new_instance(); + static Mavlink *get_instance_for_device(const char *device_name); - static int instance_count(); + mavlink_message_t *get_buffer() { return &_mavlink_buffer; } + mavlink_status_t *get_status() { return &_mavlink_status; } - static Mavlink *new_instance(); + void setProtocolVersion(uint8_t version); + uint8_t getProtocolVersion() const { return _protocol_version; }; - static Mavlink *get_instance_for_device(const char *device_name); + static int destroy_all_instances(); + static int get_status_all_instances(bool show_streams_status); + static bool serial_instance_exists(const char *device_name, Mavlink *self); - mavlink_message_t *get_buffer() { return &_mavlink_buffer; } + static bool component_was_seen(int system_id, int component_id, Mavlink &self); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); - mavlink_status_t *get_status() { return &_mavlink_status; } + bool check_events() const { return _should_check_events.load(); } + void check_events_enable() { _should_check_events.store(true); } + void check_events_disable() { _should_check_events.store(false); } - void set_protocol_version(unsigned version); + bool sending_parameters() const { return _sending_parameters.load(); } + void set_sending_parameters(bool sending) { _sending_parameters.store(sending); } - static int destroy_all_instances(); + int get_uart_fd() const { return _uart_fd; } - static int get_status_all_instances(bool show_streams_status); - - static bool serial_instance_exists(const char *device_name, Mavlink *self); - - static bool component_was_seen(int system_id, int component_id, Mavlink &self); - - static void forward_message(const mavlink_message_t *msg, Mavlink *self); - - bool check_events() const { return _should_check_events.load(); } - void check_events_enable() { _should_check_events.store(true); } - void check_events_disable() { _should_check_events.store(false); } - - bool sending_parameters() const { return _sending_parameters.load(); } - void set_sending_parameters(bool sending) { _sending_parameters.store(sending); } - - int get_uart_fd() const { return _uart_fd; } - - /** - * Get the MAVLink system id. - * - * @return The system ID of this vehicle - */ - int get_system_id() const { return mavlink_system.sysid; } - - /** - * Get the MAVLink component id. - * - * @return The component ID of this vehicle - */ - int get_component_id() const { return mavlink_system.compid; } + int get_system_id() const { return mavlink_system.sysid; } + int get_component_id() const { return mavlink_system.compid; } const char *_device_name{DEFAULT_DEVICE_NAME}; @@ -613,7 +578,7 @@ private: uint64_t _last_write_success_time{0}; uint64_t _last_write_try_time{0}; uint64_t _mavlink_start_time{0}; - int32_t _protocol_version = 0; ///< after initialization the only values are 1 and 2 + uint8_t _protocol_version = 0; ///< after initialization the only values are 1 and 2 unsigned _bytes_tx{0}; unsigned _bytes_txerr{0}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 49ce052c01..a54e425d9d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3230,9 +3230,9 @@ MavlinkReceiver::run() // If we receive a complete MAVLink 2 packet, also switch the outgoing protocol version if (!(_mavlink.get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) - && (_mavlink.get_status()->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) { + && _mavlink.getProtocolVersion() != 2) { PX4_INFO("Upgrade to MAVLink v2 because of incoming packet"); - _mavlink.set_protocol_version(2); + _mavlink.setProtocolVersion(2); } switch (_mavlink.get_mode()) {