From b1506ca6526c30c1b1daad00f38e9213ef8ff149 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 11 Jul 2019 10:31:45 +0200 Subject: [PATCH] GCS_MAVLink: pass mavlink_message_t by const reference --- libraries/GCS_MAVLink/GCS.h | 96 +++++----- libraries/GCS_MAVLink/GCS_Common.cpp | 168 +++++++++--------- libraries/GCS_MAVLink/GCS_DeviceOp.cpp | 8 +- libraries/GCS_MAVLink/GCS_Dummy.h | 2 +- libraries/GCS_MAVLink/GCS_Fence.cpp | 4 +- libraries/GCS_MAVLink/GCS_Param.cpp | 20 +-- libraries/GCS_MAVLink/GCS_Rally.cpp | 14 +- libraries/GCS_MAVLink/GCS_Signing.cpp | 4 +- libraries/GCS_MAVLink/GCS_serial_control.cpp | 4 +- libraries/GCS_MAVLink/MAVLink_routing.cpp | 76 ++++---- libraries/GCS_MAVLink/MAVLink_routing.h | 10 +- .../GCS_MAVLink/examples/routing/routing.cpp | 12 +- 12 files changed, 209 insertions(+), 209 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 1c89198ad9..73b603a31a 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -320,7 +320,7 @@ public: // packetReceived is called on any successful decode of a mavlink message virtual void packetReceived(const mavlink_status_t &status, - mavlink_message_t &msg); + const mavlink_message_t &msg); // send a mavlink_message_t out this GCS_MAVLINK connection. // Caller is responsible for ensuring space. @@ -463,7 +463,7 @@ public: send a MAVLink message to all components with this vehicle's system id This is a no-op if no routes to components have been learned */ - static void send_to_components(const mavlink_message_t* msg) { routing.send_to_components(msg); } + static void send_to_components(const mavlink_message_t &msg) { routing.send_to_components(msg); } /* allow forwarding of packets / heartbeats to be blocked as required by some components to reduce traffic @@ -505,7 +505,7 @@ protected: // overridable method to check for packet acceptance. Allows for // enforcement of GCS sysid - bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg); + bool accept_packet(const mavlink_status_t &status, const mavlink_message_t &msg); virtual AP_AdvancedFailsafe *get_advanced_failsafe() const { return nullptr; }; virtual bool set_mode(uint8_t mode) = 0; void set_ekf_origin(const Location& loc); @@ -525,11 +525,11 @@ protected: AP_Int16 streamRates[NUM_STREAMS]; virtual bool persist_streamrates() const { return false; } - void handle_request_data_stream(mavlink_message_t *msg); + void handle_request_data_stream(const mavlink_message_t &msg); - virtual void handle_command_ack(const mavlink_message_t* msg); - void handle_set_mode(mavlink_message_t* msg); - void handle_command_int(mavlink_message_t* msg); + 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); MAV_RESULT handle_command_int_do_set_home(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet); @@ -539,35 +539,35 @@ protected: MAV_RESULT handle_command_do_set_home(const mavlink_command_long_t &packet); - void handle_mission_request_list(const mavlink_message_t *msg); - void handle_mission_request(mavlink_message_t *msg); - void handle_mission_request_int(mavlink_message_t *msg); - void handle_mission_clear_all(const mavlink_message_t *msg); - virtual void handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg); - void handle_mission_count(const mavlink_message_t *msg); - void handle_mission_write_partial_list(const mavlink_message_t *msg); - void handle_mission_item(const mavlink_message_t *msg); + void handle_mission_request_list(const mavlink_message_t &msg); + void handle_mission_request(const mavlink_message_t &msg); + void handle_mission_request_int(const mavlink_message_t &msg); + void handle_mission_clear_all(const mavlink_message_t &msg); + virtual void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg); + void handle_mission_count(const mavlink_message_t &msg); + void handle_mission_write_partial_list(const mavlink_message_t &msg); + void handle_mission_item(const mavlink_message_t &msg); - void handle_common_param_message(mavlink_message_t *msg); - void handle_param_set(mavlink_message_t *msg); - void handle_param_request_list(mavlink_message_t *msg); - void handle_param_request_read(mavlink_message_t *msg); + void handle_common_param_message(const mavlink_message_t &msg); + void handle_param_set(const mavlink_message_t &msg); + void handle_param_request_list(const mavlink_message_t &msg); + void handle_param_request_read(const mavlink_message_t &msg); virtual bool params_ready() const { return true; } - virtual void handle_rc_channels_override(const mavlink_message_t *msg); - void handle_system_time_message(const mavlink_message_t *msg); - void handle_common_rally_message(mavlink_message_t *msg); - void handle_rally_fetch_point(mavlink_message_t *msg); - void handle_rally_point(mavlink_message_t *msg); - virtual void handle_mount_message(const mavlink_message_t *msg); - void handle_fence_message(mavlink_message_t *msg); - void handle_param_value(mavlink_message_t *msg); - void handle_radio_status(mavlink_message_t *msg, bool log_radio); - void handle_serial_control(const mavlink_message_t *msg); - void handle_vision_position_delta(mavlink_message_t *msg); + virtual void handle_rc_channels_override(const mavlink_message_t &msg); + void handle_system_time_message(const mavlink_message_t &msg); + void handle_common_rally_message(const mavlink_message_t &msg); + void handle_rally_fetch_point(const mavlink_message_t &msg); + void handle_rally_point(const mavlink_message_t &msg); + virtual void handle_mount_message(const mavlink_message_t &msg); + void handle_fence_message(const mavlink_message_t &msg); + void handle_param_value(const mavlink_message_t &msg); + void handle_radio_status(const mavlink_message_t &msg, bool log_radio); + void handle_serial_control(const mavlink_message_t &msg); + void handle_vision_position_delta(const mavlink_message_t &msg); - void handle_common_message(mavlink_message_t *msg); - void handle_set_gps_global_origin(const mavlink_message_t *msg); - void handle_setup_signing(const mavlink_message_t *msg); + void handle_common_message(const mavlink_message_t &msg); + void handle_set_gps_global_origin(const mavlink_message_t &msg); + void handle_setup_signing(const mavlink_message_t &msg); virtual bool should_zero_rc_outputs_on_reboot() const { return false; } MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet); @@ -580,27 +580,27 @@ protected: MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet); - void handle_send_autopilot_version(const mavlink_message_t *msg); + void handle_send_autopilot_version(const mavlink_message_t &msg); MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet); virtual void send_banner(); - void handle_device_op_read(mavlink_message_t *msg); - void handle_device_op_write(mavlink_message_t *msg); + void handle_device_op_read(const mavlink_message_t &msg); + void handle_device_op_write(const mavlink_message_t &msg); void send_timesync(); // returns the time a timesync message was most likely received: uint64_t timesync_receive_timestamp_ns() const; // returns a timestamp suitable for packing into the ts1 field of TIMESYNC: uint64_t timesync_timestamp_ns() const; - void handle_timesync(mavlink_message_t *msg); + void handle_timesync(const mavlink_message_t &msg); struct { int64_t sent_ts1; uint32_t last_sent_ms; const uint16_t interval_ms = 10000; } _timesync_request; - void handle_statustext(mavlink_message_t *msg); + void handle_statustext(const mavlink_message_t &msg); bool telemetry_delayed() const; virtual uint32_t telem_delay() const = 0; @@ -618,7 +618,7 @@ protected: MAV_RESULT handle_command_preflight_can(const mavlink_command_long_t &packet); MAV_RESULT handle_command_battery_reset(const mavlink_command_long_t &packet); - void handle_command_long(mavlink_message_t* msg); + void handle_command_long(const mavlink_message_t &msg); MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet); MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet); @@ -633,7 +633,7 @@ protected: MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet); - void handle_optical_flow(const mavlink_message_t* msg); + void handle_optical_flow(const mavlink_message_t &msg); // vehicle-overridable message send function virtual bool try_send_message(enum ap_message id); @@ -643,7 +643,7 @@ protected: bool try_send_compass_message(enum ap_message id); bool try_send_mission_message(enum ap_message id); void send_hwstatus(); - void handle_data_packet(mavlink_message_t *msg); + void handle_data_packet(const mavlink_message_t &msg); // these two methods are called after current_loc is updated: virtual int32_t global_position_int_alt() const; @@ -667,7 +667,7 @@ private: MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode); - virtual void handleMessage(mavlink_message_t * msg) = 0; + virtual void handleMessage(const mavlink_message_t &msg) = 0; MAV_RESULT handle_servorelay_message(const mavlink_command_long_t &packet); @@ -834,12 +834,12 @@ private: virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) = 0; virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) = 0; - void handle_common_mission_message(mavlink_message_t *msg); + void handle_common_mission_message(const mavlink_message_t &msg); - void handle_vicon_position_estimate(mavlink_message_t *msg); - void handle_vision_position_estimate(mavlink_message_t *msg); - void handle_global_vision_position_estimate(mavlink_message_t *msg); - void handle_att_pos_mocap(mavlink_message_t *msg); + void handle_vicon_position_estimate(const mavlink_message_t &msg); + void handle_vision_position_estimate(const mavlink_message_t &msg); + void handle_global_vision_position_estimate(const mavlink_message_t &msg); + void handle_att_pos_mocap(const mavlink_message_t &msg); void handle_common_vision_position_estimate_data(const uint64_t usec, const float x, const float y, @@ -856,7 +856,7 @@ private: const float pitch, const float yaw); - void lock_channel(mavlink_channel_t chan, bool lock); + void lock_channel(const mavlink_channel_t chan, bool lock); /* correct an offboard timestamp in microseconds to a local time diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index cfff492702..8d46e2856c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -524,23 +524,23 @@ MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE missi } // handle a request for the number of items we have stored for a mission type: -void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t &msg) { // decode mavlink_mission_request_list_t packet; - mavlink_msg_mission_request_list_decode(msg, &packet); + mavlink_msg_mission_request_list_decode(&msg, &packet); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type); if (prot == nullptr) { mavlink_msg_mission_ack_send(chan, - msg->sysid, - msg->compid, + msg.sysid, + msg.compid, MAV_MISSION_UNSUPPORTED, packet.mission_type); return; } - prot->handle_mission_request_list(*this, packet, *msg); + prot->handle_mission_request_list(*this, packet, msg); } void MissionItemProtocol::handle_mission_request_list( @@ -602,17 +602,17 @@ void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link, /* handle a MISSION_REQUEST mavlink packet */ -void GCS_MAVLINK::handle_mission_request_int(mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_request_int(const mavlink_message_t &msg) { // decode mavlink_mission_request_int_t packet; - mavlink_msg_mission_request_int_decode(msg, &packet); + mavlink_msg_mission_request_int_decode(&msg, &packet); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type); if (prot == nullptr) { return; } - prot->handle_mission_request_int(*this, packet, *msg); + prot->handle_mission_request_int(*this, packet, msg); } MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_link, @@ -657,17 +657,17 @@ MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_l return MAV_MISSION_ACCEPTED; } -void GCS_MAVLINK::handle_mission_request(mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg) { // decode mavlink_mission_request_t packet; - mavlink_msg_mission_request_decode(msg, &packet); + mavlink_msg_mission_request_decode(&msg, &packet); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type); if (prot == nullptr) { return; } - prot->handle_mission_request(*this, packet, *msg); + prot->handle_mission_request(*this, packet, msg); } void MissionItemProtocol::convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(const mavlink_mission_request_t &request, mavlink_mission_request_int_t &request_int) @@ -713,11 +713,11 @@ void MissionItemProtocol::handle_mission_request(const GCS_MAVLINK &_link, /* handle a MISSION_SET_CURRENT mavlink packet */ -void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg) { // decode mavlink_mission_set_current_t packet; - mavlink_msg_mission_set_current_decode(msg, &packet); + mavlink_msg_mission_set_current_decode(&msg, &packet); // set current command if (mission.set_current_cmd(packet.seq)) { @@ -728,23 +728,23 @@ void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, mavlink_messag /* handle a MISSION_COUNT mavlink packet */ -void GCS_MAVLINK::handle_mission_count(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_count(const mavlink_message_t &msg) { // decode mavlink_mission_count_t packet; - mavlink_msg_mission_count_decode(msg, &packet); + mavlink_msg_mission_count_decode(&msg, &packet); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type); if (prot == nullptr) { mavlink_msg_mission_ack_send(chan, - msg->sysid, - msg->compid, + msg.sysid, + msg.compid, MAV_MISSION_UNSUPPORTED, packet.mission_type); return; } - prot->handle_mission_count(*this, packet, *msg); + prot->handle_mission_count(*this, packet, msg); } void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link, @@ -810,20 +810,20 @@ void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &pack /* handle a MISSION_CLEAR_ALL mavlink packet */ -void GCS_MAVLINK::handle_mission_clear_all(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_clear_all(const mavlink_message_t &msg) { // decode mavlink_mission_clear_all_t packet; - mavlink_msg_mission_clear_all_decode(msg, &packet); + mavlink_msg_mission_clear_all_decode(&msg, &packet); const MAV_MISSION_TYPE mission_type = (MAV_MISSION_TYPE)packet.mission_type; MissionItemProtocol *prot = gcs().get_prot_for_mission_type(mission_type); if (prot == nullptr) { - send_mission_ack(*msg, mission_type, MAV_MISSION_UNSUPPORTED); + send_mission_ack(msg, mission_type, MAV_MISSION_UNSUPPORTED); return; } - prot->handle_mission_clear_all(*this, *msg); + prot->handle_mission_clear_all(*this, msg); } bool MissionItemProtocol_Waypoints::clear_all_items() @@ -857,18 +857,18 @@ bool GCS_MAVLINK::requesting_mission_items() const return false; } -void GCS_MAVLINK::handle_mission_write_partial_list(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_write_partial_list(const mavlink_message_t &msg) { // decode mavlink_mission_write_partial_list_t packet; - mavlink_msg_mission_write_partial_list_decode(msg, &packet); + mavlink_msg_mission_write_partial_list_decode(&msg, &packet); MissionItemProtocol *use_prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type); if (use_prot == nullptr) { - send_mission_ack(*msg, (MAV_MISSION_TYPE)packet.mission_type, MAV_MISSION_UNSUPPORTED); + send_mission_ack(msg, (MAV_MISSION_TYPE)packet.mission_type, MAV_MISSION_UNSUPPORTED); return; } - use_prot->handle_mission_write_partial_list(*this, *msg, packet); + use_prot->handle_mission_write_partial_list(*this, msg, packet); } void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link, @@ -892,7 +892,7 @@ void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link, /* pass mavlink messages to the AP_Mount singleton */ -void GCS_MAVLINK::handle_mount_message(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_mount_message(const mavlink_message_t &msg) { AP_Mount *mount = AP::mount(); if (mount == nullptr) { @@ -904,7 +904,7 @@ void GCS_MAVLINK::handle_mount_message(const mavlink_message_t *msg) /* pass parameter value messages through to mount library */ -void GCS_MAVLINK::handle_param_value(mavlink_message_t *msg) +void GCS_MAVLINK::handle_param_value(const mavlink_message_t &msg) { AP_Mount *mount = AP::mount(); if (mount == nullptr) { @@ -927,10 +927,10 @@ void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *fmt, ...) const va_end(arg_list); } -void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, bool log_radio) +void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg, bool log_radio) { mavlink_radio_t packet; - mavlink_msg_radio_decode(msg, &packet); + mavlink_msg_radio_decode(&msg, &packet); // record if the GCS has been receiving radio messages from // the aircraft @@ -971,21 +971,21 @@ void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, bool log_radio) handle an incoming mission item return true if this is the last mission item, otherwise false */ -void GCS_MAVLINK::handle_mission_item(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg) { // TODO: rename packet to mission_item_int mavlink_mission_item_int_t packet; - if (msg->msgid == MAVLINK_MSG_ID_MISSION_ITEM) { + if (msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM) { mavlink_mission_item_t mission_item; - mavlink_msg_mission_item_decode(msg, &mission_item); + mavlink_msg_mission_item_decode(&msg, &mission_item); MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, packet); if (ret != MAV_MISSION_ACCEPTED) { const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type; - send_mission_ack(*msg, type, ret); + send_mission_ack(msg, type, ret); return; } } else { - mavlink_msg_mission_item_int_decode(msg, &packet); + mavlink_msg_mission_item_int_decode(&msg, &packet); } const uint8_t current = packet.current; const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type; @@ -995,7 +995,7 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t *msg) MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(packet, cmd); if (result != MAV_MISSION_ACCEPTED) { //decode failed - send_mission_ack(*msg, MAV_MISSION_TYPE_MISSION, result); + send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result); return; } // guided or change-alt @@ -1012,23 +1012,23 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t *msg) // verify we recevied the command result = MAV_MISSION_ACCEPTED; } - send_mission_ack(*msg, MAV_MISSION_TYPE_MISSION, result); + send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result); return; } // not a guided-mode reqest MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type); if (prot == nullptr) { - send_mission_ack(*msg, type, MAV_MISSION_UNSUPPORTED); + send_mission_ack(msg, type, MAV_MISSION_UNSUPPORTED); return; } if (!prot->receiving) { - send_mission_ack(*msg, type, MAV_MISSION_ERROR); + send_mission_ack(msg, type, MAV_MISSION_ERROR); return; } - prot->handle_mission_item(*msg, packet); + prot->handle_mission_item(msg, packet); } MAV_MISSION_RESULT MissionItemProtocol_Waypoints::replace_item(const mavlink_mission_item_int_t &mission_item_int) @@ -1693,7 +1693,7 @@ void GCS_MAVLINK::send_message(enum ap_message id) } void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, - mavlink_message_t &msg) + const mavlink_message_t &msg) { // we exclude radio packets because we historically used this to // make it possible to use the CLI over the radio @@ -1714,7 +1714,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; } } - if (!routing.check_and_forward(chan, &msg)) { + if (!routing.check_and_forward(chan, msg)) { // the routing code has indicated we should not handle this packet locally return; } @@ -1722,7 +1722,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, // e.g. enforce-sysid says we shouldn't look at this packet return; } - handleMessage(&msg); + handleMessage(msg); } void @@ -2340,10 +2340,10 @@ void GCS_MAVLINK::send_battery2() /* handle a SET_MODE MAVLink message */ -void GCS_MAVLINK::handle_set_mode(mavlink_message_t* msg) +void GCS_MAVLINK::handle_set_mode(const mavlink_message_t &msg) { mavlink_set_mode_t packet; - mavlink_msg_set_mode_decode(msg, &packet); + mavlink_msg_set_mode_decode(&msg, &packet); const MAV_MODE _base_mode = (MAV_MODE)packet.base_mode; const uint32_t _custom_mode = packet.custom_mode; @@ -2904,11 +2904,11 @@ uint64_t GCS_MAVLINK::timesync_timestamp_ns() const return a timesync request Sends back ts1 as received, and tc1 is the local timestamp in usec */ -void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg) +void GCS_MAVLINK::handle_timesync(const mavlink_message_t &msg) { // decode incoming timesync message mavlink_timesync_t tsync; - mavlink_msg_timesync_decode(msg, &tsync); + mavlink_msg_timesync_decode(&msg, &tsync); if (tsync.tc1 != 0) { // this is a response to a timesync request @@ -2921,7 +2921,7 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg) #if 0 gcs().send_text(MAV_SEVERITY_INFO, "timesync response sysid=%u (latency=%fms)", - msg->sysid, + msg.sysid, round_trip_time_us*0.001f); #endif AP_Logger *logger = AP_Logger::get_singleton(); @@ -2933,7 +2933,7 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg) "F-F", "QBQ", AP_HAL::micros64(), - msg->sysid, + msg.sysid, round_trip_time_us ); } @@ -2973,7 +2973,7 @@ void GCS_MAVLINK::send_timesync() ); } -void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg) +void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) { AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr) { @@ -2981,18 +2981,18 @@ void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg) } mavlink_statustext_t packet; - mavlink_msg_statustext_decode(msg, &packet); + mavlink_msg_statustext_decode(&msg, &packet); const uint8_t max_prefix_len = 20; const uint8_t text_len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+max_prefix_len; char text[text_len] = { 'G','C','S',':'}; uint8_t offset = strlen(text); - if (msg->sysid != sysid_my_gcs()) { + if (msg.sysid != sysid_my_gcs()) { offset = hal.util->snprintf(text, max_prefix_len, "SRC=%u/%u:", - msg->sysid, - msg->compid); + msg.sysid, + msg.compid); } memcpy(&text[offset], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); @@ -3001,10 +3001,10 @@ void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg) } -void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg) { mavlink_system_time_t packet; - mavlink_msg_system_time_decode(msg, &packet); + mavlink_msg_system_time_decode(&msg, &packet); AP::rtc().set_utc_usec(packet.time_unix_usec, AP_RTC::SOURCE_MAVLINK_SYSTEM_TIME); } @@ -3079,10 +3079,10 @@ void GCS_MAVLINK::set_ekf_origin(const Location& loc) } } -void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t &msg) { mavlink_set_gps_global_origin_t packet; - mavlink_msg_set_gps_global_origin_decode(msg, &packet); + mavlink_msg_set_gps_global_origin_decode(&msg, &packet); // sanity check location if (!check_latlng(packet.latitude, packet.longitude)) { @@ -3100,11 +3100,11 @@ void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t *msg) /* handle a DATA96 message */ -void GCS_MAVLINK::handle_data_packet(mavlink_message_t *msg) +void GCS_MAVLINK::handle_data_packet(const mavlink_message_t &msg) { #if HAL_RCINPUT_WITH_AP_RADIO mavlink_data96_t m; - mavlink_msg_data96_decode(msg, &m); + mavlink_msg_data96_decode(&msg, &m); switch (m.type) { case 42: case 43: { @@ -3122,7 +3122,7 @@ void GCS_MAVLINK::handle_data_packet(mavlink_message_t *msg) #endif } -void GCS_MAVLINK::handle_vision_position_delta(mavlink_message_t *msg) +void GCS_MAVLINK::handle_vision_position_delta(const mavlink_message_t &msg) { AP_VisualOdom *visual_odom = AP::visualodom(); if (visual_odom == nullptr) { @@ -3131,28 +3131,28 @@ void GCS_MAVLINK::handle_vision_position_delta(mavlink_message_t *msg) visual_odom->handle_msg(msg); } -void GCS_MAVLINK::handle_vision_position_estimate(mavlink_message_t *msg) +void GCS_MAVLINK::handle_vision_position_estimate(const mavlink_message_t &msg) { mavlink_vision_position_estimate_t m; - mavlink_msg_vision_position_estimate_decode(msg, &m); + mavlink_msg_vision_position_estimate_decode(&msg, &m); handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, PAYLOAD_SIZE(chan, VISION_POSITION_ESTIMATE)); } -void GCS_MAVLINK::handle_global_vision_position_estimate(mavlink_message_t *msg) +void GCS_MAVLINK::handle_global_vision_position_estimate(const mavlink_message_t &msg) { mavlink_global_vision_position_estimate_t m; - mavlink_msg_global_vision_position_estimate_decode(msg, &m); + mavlink_msg_global_vision_position_estimate_decode(&msg, &m); handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, PAYLOAD_SIZE(chan, GLOBAL_VISION_POSITION_ESTIMATE)); } -void GCS_MAVLINK::handle_vicon_position_estimate(mavlink_message_t *msg) +void GCS_MAVLINK::handle_vicon_position_estimate(const mavlink_message_t &msg) { mavlink_vicon_position_estimate_t m; - mavlink_msg_vicon_position_estimate_decode(msg, &m); + mavlink_msg_vicon_position_estimate_decode(&msg, &m); handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE)); @@ -3218,10 +3218,10 @@ void GCS_MAVLINK::log_vision_position_estimate_data(const uint64_t usec, (double)(yaw * RAD_TO_DEG)); } -void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg) +void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg) { mavlink_att_pos_mocap_t m; - mavlink_msg_att_pos_mocap_decode(msg, &m); + mavlink_msg_att_pos_mocap_decode(&msg, &m); // sensor assumed to be at 0,0,0 body-frame; need parameters for this? const Vector3f sensor_offset = {}; @@ -3254,7 +3254,7 @@ void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg) log_vision_position_estimate_data(m.time_usec, m.x, m.y, m.z, roll, pitch, yaw); } -void GCS_MAVLINK::handle_command_ack(const mavlink_message_t* msg) +void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg) { AP_AccelCal *accelcal = AP::ins().get_acal(); if (accelcal != nullptr) { @@ -3264,16 +3264,16 @@ void GCS_MAVLINK::handle_command_ack(const mavlink_message_t* msg) // allow override of RC channel values for HIL or for complete GCS // control of switch position and RC PWM values. -void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg) { - if(msg->sysid != sysid_my_gcs()) { + if(msg.sysid != sysid_my_gcs()) { return; // Only accept control from our gcs } const uint32_t tnow = AP_HAL::millis(); mavlink_rc_channels_override_t packet; - mavlink_msg_rc_channels_override_decode(msg, &packet); + mavlink_msg_rc_channels_override_decode(&msg, &packet); const uint16_t override_data[] = { packet.chan1_raw, @@ -3304,7 +3304,7 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t *msg) // allow override of RC channel values for HIL or for complete GCS // control of switch position and RC PWM values. -void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg) { OpticalFlow *optflow = AP::opticalflow(); if (optflow == nullptr) { @@ -3316,9 +3316,9 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t *msg) /* handle messages which don't require vehicle specific data */ -void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) +void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) { - switch (msg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_COMMAND_ACK: { handle_command_ack(msg); break; @@ -3485,13 +3485,13 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) } } -void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg) +void GCS_MAVLINK::handle_common_mission_message(const mavlink_message_t &msg) { AP_Mission *_mission = AP::mission(); if (_mission == nullptr) { return; } - switch (msg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38 { handle_mission_write_partial_list(msg); @@ -3545,7 +3545,7 @@ void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg) } } -void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t &msg) { send_autopilot_version(); } @@ -4034,11 +4034,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t return result; } -void GCS_MAVLINK::handle_command_long(mavlink_message_t *msg) +void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) { // decode packet mavlink_command_long_t packet; - mavlink_msg_command_long_decode(msg, &packet); + mavlink_msg_command_long_decode(&msg, &packet); hal.util->persistent_data.last_mavlink_cmd = packet.command; @@ -4155,11 +4155,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return MAV_RESULT_UNSUPPORTED; } -void GCS_MAVLINK::handle_command_int(mavlink_message_t *msg) +void GCS_MAVLINK::handle_command_int(const mavlink_message_t &msg) { // decode packet mavlink_command_int_t packet; - mavlink_msg_command_int_decode(msg, &packet); + mavlink_msg_command_int_decode(&msg, &packet); hal.util->persistent_data.last_mavlink_cmd = packet.command; @@ -4825,7 +4825,7 @@ uint32_t GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(uint64_t offboard_us return true if we will accept this packet. Used to implement SYSID_ENFORCE */ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, - mavlink_message_t &msg) + const mavlink_message_t &msg) { if (msg.sysid == mavlink_system.sysid) { // accept packets from our own components diff --git a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp index 5b7ad42c13..4b5abed014 100644 --- a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp +++ b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp @@ -26,10 +26,10 @@ extern const AP_HAL::HAL& hal; /* handle DEVICE_OP_READ message */ -void GCS_MAVLINK::handle_device_op_read(mavlink_message_t *msg) +void GCS_MAVLINK::handle_device_op_read(const mavlink_message_t &msg) { mavlink_device_op_read_t packet; - mavlink_msg_device_op_read_decode(msg, &packet); + mavlink_msg_device_op_read_decode(&msg, &packet); AP_HAL::OwnPtr dev = nullptr; uint8_t retcode = 0; uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {}; @@ -78,10 +78,10 @@ fail: /* handle DEVICE_OP_WRITE message */ -void GCS_MAVLINK::handle_device_op_write(mavlink_message_t *msg) +void GCS_MAVLINK::handle_device_op_write(const mavlink_message_t &msg) { mavlink_device_op_write_t packet; - mavlink_msg_device_op_write_decode(msg, &packet); + mavlink_msg_device_op_write_decode(&msg, &packet); AP_HAL::OwnPtr dev = nullptr; uint8_t retcode = 0; diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 652f8bff69..af8a9894a7 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -24,7 +24,7 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {}; class GCS_MAVLINK_Dummy : public GCS_MAVLINK { uint32_t telem_delay() const override { return 0; } - void handleMessage(mavlink_message_t * msg) override {} + void handleMessage(const mavlink_message_t &msg) override {} bool try_send_message(enum ap_message id) override { return true; } bool handle_guided_request(AP_Mission::Mission_Command &cmd) override { return true; } void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override {} diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index 22849dd61e..886e531c47 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -21,14 +21,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_lon } } -void GCS_MAVLINK::handle_fence_message(mavlink_message_t *msg) +void GCS_MAVLINK::handle_fence_message(const mavlink_message_t &msg) { AC_Fence *fence = AP::fence(); if (fence == nullptr) { return; } // send or receive fence points with GCS - switch (msg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_FENCE_POINT: case MAVLINK_MSG_ID_FENCE_FETCH_POINT: fence->handle_msg(*this, msg); diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index 127e4c5482..748058deff 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -125,10 +125,10 @@ bool GCS_MAVLINK::have_flow_control(void) save==false so we don't want the save to happen when the user connects the ground station. */ -void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg) +void GCS_MAVLINK::handle_request_data_stream(const mavlink_message_t &msg) { mavlink_request_data_stream_t packet; - mavlink_msg_request_data_stream_decode(msg, &packet); + mavlink_msg_request_data_stream_decode(&msg, &packet); int16_t freq = 0; // packet frequency @@ -202,14 +202,14 @@ void GCS_MAVLINK::handle_request_data_stream(mavlink_message_t *msg) } } -void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg) +void GCS_MAVLINK::handle_param_request_list(const mavlink_message_t &msg) { if (!params_ready()) { return; } mavlink_param_request_list_t packet; - mavlink_msg_param_request_list_decode(msg, &packet); + mavlink_msg_param_request_list_decode(&msg, &packet); // requesting parameters is a convenient way to get extra information send_banner(); @@ -221,7 +221,7 @@ void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg) _queued_parameter_send_time_ms = AP_HAL::millis(); // avoid initial flooding } -void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg) +void GCS_MAVLINK::handle_param_request_read(const mavlink_message_t &msg) { if (param_requests.space() == 0) { // we can't process this right now, drop it @@ -229,7 +229,7 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg) } mavlink_param_request_read_t packet; - mavlink_msg_param_request_read_decode(msg, &packet); + mavlink_msg_param_request_read_decode(&msg, &packet); /* we reserve some space for sending parameters if the client ever @@ -259,10 +259,10 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg) } } -void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg) +void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg) { mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); + mavlink_msg_param_set_decode(&msg, &packet); enum ap_var_type var_type; // set parameter @@ -417,9 +417,9 @@ uint8_t GCS_MAVLINK::send_parameter_async_replies() return async_replies_sent_count; } -void GCS_MAVLINK::handle_common_param_message(mavlink_message_t *msg) +void GCS_MAVLINK::handle_common_param_message(const mavlink_message_t &msg) { - switch (msg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: handle_param_request_list(msg); break; diff --git a/libraries/GCS_MAVLink/GCS_Rally.cpp b/libraries/GCS_MAVLink/GCS_Rally.cpp index 84df57ac64..8534793f2d 100644 --- a/libraries/GCS_MAVLink/GCS_Rally.cpp +++ b/libraries/GCS_MAVLink/GCS_Rally.cpp @@ -19,7 +19,7 @@ #include #include -void GCS_MAVLINK::handle_rally_point(mavlink_message_t *msg) +void GCS_MAVLINK::handle_rally_point(const mavlink_message_t &msg) { AP_Rally *r = AP::rally(); if (r == nullptr) { @@ -27,7 +27,7 @@ void GCS_MAVLINK::handle_rally_point(mavlink_message_t *msg) } mavlink_rally_point_t packet; - mavlink_msg_rally_point_decode(msg, &packet); + mavlink_msg_rally_point_decode(&msg, &packet); if (packet.idx >= r->get_rally_total() || packet.idx >= r->get_rally_max()) { @@ -58,7 +58,7 @@ void GCS_MAVLINK::handle_rally_point(mavlink_message_t *msg) } } -void GCS_MAVLINK::handle_rally_fetch_point(mavlink_message_t *msg) +void GCS_MAVLINK::handle_rally_fetch_point(const mavlink_message_t &msg) { AP_Rally *r = AP::rally(); if (r == nullptr) { @@ -66,7 +66,7 @@ void GCS_MAVLINK::handle_rally_fetch_point(mavlink_message_t *msg) } mavlink_rally_fetch_point_t packet; - mavlink_msg_rally_fetch_point_decode(msg, &packet); + mavlink_msg_rally_fetch_point_decode(&msg, &packet); if (packet.idx > r->get_rally_total()) { send_text(MAV_SEVERITY_WARNING, "Bad rally point ID"); @@ -79,15 +79,15 @@ void GCS_MAVLINK::handle_rally_fetch_point(mavlink_message_t *msg) return; } - mavlink_msg_rally_point_send(chan, msg->sysid, msg->compid, packet.idx, + mavlink_msg_rally_point_send(chan, msg.sysid, msg.compid, packet.idx, r->get_rally_total(), rally_point.lat, rally_point.lng, rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.flags); } -void GCS_MAVLINK::handle_common_rally_message(mavlink_message_t *msg) +void GCS_MAVLINK::handle_common_rally_message(const mavlink_message_t &msg) { - switch (msg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_RALLY_POINT: handle_rally_point(msg); break; diff --git a/libraries/GCS_MAVLink/GCS_Signing.cpp b/libraries/GCS_MAVLink/GCS_Signing.cpp index d52b51f912..5d0023c369 100644 --- a/libraries/GCS_MAVLink/GCS_Signing.cpp +++ b/libraries/GCS_MAVLink/GCS_Signing.cpp @@ -63,7 +63,7 @@ bool GCS_MAVLINK::signing_key_load(struct SigningKey &key) /* handle a setup_signing message */ -void GCS_MAVLINK::handle_setup_signing(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_setup_signing(const mavlink_message_t &msg) { // setting up signing key when armed generally not useful / // possibly not a good idea @@ -74,7 +74,7 @@ void GCS_MAVLINK::handle_setup_signing(const mavlink_message_t *msg) // decode mavlink_setup_signing_t packet; - mavlink_msg_setup_signing_decode(msg, &packet); + mavlink_msg_setup_signing_decode(&msg, &packet); struct SigningKey key; key.magic = SIGNING_KEY_MAGIC; diff --git a/libraries/GCS_MAVLink/GCS_serial_control.cpp b/libraries/GCS_MAVLink/GCS_serial_control.cpp index a5db2efb3a..3062face6f 100644 --- a/libraries/GCS_MAVLink/GCS_serial_control.cpp +++ b/libraries/GCS_MAVLink/GCS_serial_control.cpp @@ -28,10 +28,10 @@ extern const AP_HAL::HAL& hal; /** handle a SERIAL_CONTROL message */ -void GCS_MAVLINK::handle_serial_control(const mavlink_message_t *msg) +void GCS_MAVLINK::handle_serial_control(const mavlink_message_t &msg) { mavlink_serial_control_t packet; - mavlink_msg_serial_control_decode(msg, &packet); + mavlink_msg_serial_control_decode(&msg, &packet); AP_HAL::UARTDriver *port = nullptr; AP_HAL::BetterStream *stream = nullptr; diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp index f814dfd76e..d318513813 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.cpp +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -88,31 +88,31 @@ detect a reset of the flight controller, which implies a reset of its routing table. */ -bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t* msg) +bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t &msg) { // handle the case of loopback of our own messages, due to // incorrect serial configuration. - if (msg->sysid == mavlink_system.sysid && - msg->compid == mavlink_system.compid) { + if (msg.sysid == mavlink_system.sysid && + msg.compid == mavlink_system.compid) { return true; } // learn new routes learn_route(in_channel, msg); - if (msg->msgid == MAVLINK_MSG_ID_RADIO || - msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + if (msg.msgid == MAVLINK_MSG_ID_RADIO || + msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { // don't forward RADIO packets return true; } - if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) { + if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { // heartbeat needs special handling handle_heartbeat(in_channel, msg); return true; } - if (msg->msgid == MAVLINK_MSG_ID_ADSB_VEHICLE) { + if (msg.msgid == MAVLINK_MSG_ID_ADSB_VEHICLE) { // ADSB packets are not forwarded, they have their own stream rate return true; } @@ -154,17 +154,17 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl if (in_channel != routes[i].channel && !sent_to_chan[routes[i].channel]) { - if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) + + if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg.len) + GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) { #if ROUTING_DEBUG ::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n", - msg->msgid, + msg.msgid, (unsigned)in_channel, (unsigned)routes[i].channel, (int)target_system, (int)target_component); #endif - _mavlink_resend_uart(routes[i].channel, msg); + _mavlink_resend_uart(routes[i].channel, &msg); } sent_to_chan[routes[i].channel] = true; forwarded = true; @@ -184,7 +184,7 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl This is a no-op if no routes to components have been learned */ -void MAVLink_routing::send_to_components(const mavlink_message_t* msg) +void MAVLink_routing::send_to_components(const mavlink_message_t &msg) { bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS]; memset(sent_to_chan, 0, sizeof(sent_to_chan)); @@ -192,16 +192,16 @@ void MAVLink_routing::send_to_components(const mavlink_message_t* msg) // check learned routes for (uint8_t i=0; i= ((uint16_t)msg->len) + + if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg.len) + GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) { #if ROUTING_DEBUG ::printf("send msg %u on chan %u sysid=%u compid=%u\n", - msg->msgid, + msg.msgid, (unsigned)routes[i].channel, (unsigned)routes[i].sysid, (unsigned)routes[i].compid); #endif - _mavlink_resend_uart(routes[i].channel, msg); + _mavlink_resend_uart(routes[i].channel, &msg); sent_to_chan[routes[i].channel] = true; } } @@ -231,36 +231,36 @@ bool MAVLink_routing::find_by_mavtype(uint8_t mavtype, uint8_t &sysid, uint8_t & /* see if the message is for a new route and learn it */ -void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_message_t* msg) +void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_message_t &msg) { uint8_t i; - if (msg->sysid == 0 || - (msg->sysid == mavlink_system.sysid && - msg->compid == mavlink_system.compid)) { + if (msg.sysid == 0 || + (msg.sysid == mavlink_system.sysid && + msg.compid == mavlink_system.compid)) { return; } for (i=0; isysid && - routes[i].compid == msg->compid && + if (routes[i].sysid == msg.sysid && + routes[i].compid == msg.compid && routes[i].channel == in_channel) { - if (routes[i].mavtype == 0 && msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) { - routes[i].mavtype = mavlink_msg_heartbeat_get_type(msg); + if (routes[i].mavtype == 0 && msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { + routes[i].mavtype = mavlink_msg_heartbeat_get_type(&msg); } break; } } if (i == num_routes && isysid; - routes[i].compid = msg->compid; + routes[i].sysid = msg.sysid; + routes[i].compid = msg.compid; routes[i].channel = in_channel; - if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) { - routes[i].mavtype = mavlink_msg_heartbeat_get_type(msg); + if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { + routes[i].mavtype = mavlink_msg_heartbeat_get_type(&msg); } num_routes++; #if ROUTING_DEBUG ::printf("learned route %u %u via %u\n", - (unsigned)msg->sysid, - (unsigned)msg->compid, + (unsigned)msg.sysid, + (unsigned)msg.compid, (unsigned)in_channel); #endif } @@ -272,7 +272,7 @@ void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_me propagation heartbeat messages need to be forwarded on all channels except channels where the sysid/compid of the heartbeat could come from */ -void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t* msg) +void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t &msg) { uint16_t mask = GCS_MAVLINK::active_channel_mask(); @@ -285,7 +285,7 @@ void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavli // mask out channels that are known sources for this sysid/compid for (uint8_t i=0; isysid && routes[i].compid == msg->compid) { + if (routes[i].sysid == msg.sysid && routes[i].compid == msg.compid) { mask &= ~(1U<<((unsigned)(routes[i].channel-MAVLINK_COMM_0))); } } @@ -299,16 +299,16 @@ void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavli for (uint8_t i=0; i= ((uint16_t)msg->len) + + if (comm_get_txspace(channel) >= ((uint16_t)msg.len) + GCS_MAVLINK::packet_overhead_chan(channel)) { #if ROUTING_DEBUG ::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n", (unsigned)in_channel, (unsigned)channel, - (unsigned)msg->sysid, - (unsigned)msg->compid); + (unsigned)msg.sysid, + (unsigned)msg.compid); #endif - _mavlink_resend_uart(channel, msg); + _mavlink_resend_uart(channel, &msg); } } } @@ -320,17 +320,17 @@ void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavli that the caller can set them to -1 and know when a sysid or compid target is found in the message */ -void MAVLink_routing::get_targets(const mavlink_message_t* msg, int16_t &sysid, int16_t &compid) +void MAVLink_routing::get_targets(const mavlink_message_t &msg, int16_t &sysid, int16_t &compid) { - const mavlink_msg_entry_t *msg_entry = mavlink_get_msg_entry(msg->msgid); + const mavlink_msg_entry_t *msg_entry = mavlink_get_msg_entry(msg.msgid); if (msg_entry == nullptr) { return; } if (msg_entry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { - sysid = _MAV_RETURN_uint8_t(msg, msg_entry->target_system_ofs); + sysid = _MAV_RETURN_uint8_t(&msg, msg_entry->target_system_ofs); } if (msg_entry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { - compid = _MAV_RETURN_uint8_t(msg, msg_entry->target_component_ofs); + compid = _MAV_RETURN_uint8_t(&msg, msg_entry->target_component_ofs); } } diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h index 07505a18db..b3b2c80fd4 100644 --- a/libraries/GCS_MAVLink/MAVLink_routing.h +++ b/libraries/GCS_MAVLink/MAVLink_routing.h @@ -27,13 +27,13 @@ public: This returns true if the message should be processed locally */ - bool check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t* msg); + bool check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t &msg); /* send a MAVLink message to all components with this vehicle's system id This is a no-op if no routes to components have been learned */ - void send_to_components(const mavlink_message_t* msg); + void send_to_components(const mavlink_message_t &msg); /* search for the first vehicle or component in the routing table with given mav_type and retrieve it's sysid, compid and channel @@ -56,11 +56,11 @@ private: uint8_t no_route_mask; // learn new routes - void learn_route(mavlink_channel_t in_channel, const mavlink_message_t* msg); + void learn_route(mavlink_channel_t in_channel, const mavlink_message_t &msg); // extract target sysid and compid from a message - void get_targets(const mavlink_message_t* msg, int16_t &sysid, int16_t &compid); + void get_targets(const mavlink_message_t &msg, int16_t &sysid, int16_t &compid); // special handling for heartbeat messages - void handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t* msg); + void handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t &msg); }; diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index 5ea5ec3f12..1fb833a400 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -39,7 +39,7 @@ void loop(void) mavlink_msg_heartbeat_encode(3, 1, &msg, &heartbeat); - if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { + if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { hal.console->printf("heartbeat should be processed locally\n"); err_count++; } @@ -47,7 +47,7 @@ void loop(void) // incoming non-targetted message mavlink_attitude_t attitude = {0}; mavlink_msg_attitude_encode(3, 1, &msg, &attitude); - if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { + if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { hal.console->printf("attitude should be processed locally\n"); err_count++; } @@ -57,7 +57,7 @@ void loop(void) param_set.target_system = mavlink_system.sysid+1; param_set.target_component = mavlink_system.compid; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); - if (routing.check_and_forward(MAVLINK_COMM_0, &msg)) { + if (routing.check_and_forward(MAVLINK_COMM_0, msg)) { hal.console->printf("param set 1 should not be processed locally\n"); err_count++; } @@ -66,7 +66,7 @@ void loop(void) param_set.target_system = mavlink_system.sysid; param_set.target_component = mavlink_system.compid; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); - if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { + if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { hal.console->printf("param set 2 should be processed locally\n"); err_count++; } @@ -76,7 +76,7 @@ void loop(void) param_set.target_system = mavlink_system.sysid; param_set.target_component = mavlink_system.compid+1; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); - if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { + if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { hal.console->printf("param set 3 should be processed locally\n"); err_count++; } @@ -85,7 +85,7 @@ void loop(void) param_set.target_system = 0; param_set.target_component = mavlink_system.compid+1; mavlink_msg_param_set_encode(3, 1, &msg, ¶m_set); - if (!routing.check_and_forward(MAVLINK_COMM_0, &msg)) { + if (!routing.check_and_forward(MAVLINK_COMM_0, msg)) { hal.console->printf("param set 4 should be processed locally\n"); err_count++; }