Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: pass mavlink_message_t by const reference

Browse Source
This commit is contained in:
Pierre Kancir 2019-07-11 10:31:45 +02:00 committed by Peter Barker
parent 9a734c1fc7
commit b1506ca652
12 changed files with 209 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libraries/GCS_MAVLink/GCS.h
View File

@ -320,7 +320,7 @@ public:
// packetReceived is called on any successful decode of a mavlink message // packetReceived is called on any successful decode of a mavlink message
virtual void packetReceived(const mavlink_status_t &status, 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. // send a mavlink_message_t out this GCS_MAVLINK connection.
// Caller is responsible for ensuring space. // Caller is responsible for ensuring space.
@ -463,7 +463,7 @@ public:
send a MAVLink message to all components with this vehicle's system id 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 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 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 // overridable method to check for packet acceptance. Allows for
// enforcement of GCS sysid // 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 AP_AdvancedFailsafe *get_advanced_failsafe() const { return nullptr; };
virtual bool set_mode(uint8_t mode) = 0; virtual bool set_mode(uint8_t mode) = 0;
void set_ekf_origin(const Location& loc); void set_ekf_origin(const Location& loc);
@ -525,11 +525,11 @@ protected:
AP_Int16 streamRates[NUM_STREAMS]; AP_Int16 streamRates[NUM_STREAMS];
virtual bool persist_streamrates() const { return false; } 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); virtual void handle_command_ack(const mavlink_message_t &msg);
void handle_set_mode(mavlink_message_t* msg); void handle_set_mode(const mavlink_message_t &msg);
void handle_command_int(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); 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); 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); 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_list(const mavlink_message_t &msg);
void handle_mission_request(mavlink_message_t *msg); void handle_mission_request(const mavlink_message_t &msg);
void handle_mission_request_int(mavlink_message_t *msg); void handle_mission_request_int(const mavlink_message_t &msg);
void handle_mission_clear_all(const 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); 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_count(const mavlink_message_t &msg);
void handle_mission_write_partial_list(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_item(const mavlink_message_t &msg);
void handle_common_param_message(mavlink_message_t *msg); void handle_common_param_message(const mavlink_message_t &msg);
void handle_param_set(mavlink_message_t *msg); void handle_param_set(const mavlink_message_t &msg);
void handle_param_request_list(mavlink_message_t *msg); void handle_param_request_list(const mavlink_message_t &msg);
void handle_param_request_read(mavlink_message_t *msg); void handle_param_request_read(const mavlink_message_t &msg);
virtual bool params_ready() const { return true; } virtual bool params_ready() const { return true; }
virtual void handle_rc_channels_override(const 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_system_time_message(const mavlink_message_t &msg);
void handle_common_rally_message(mavlink_message_t *msg); void handle_common_rally_message(const mavlink_message_t &msg);
void handle_rally_fetch_point(mavlink_message_t *msg); void handle_rally_fetch_point(const mavlink_message_t &msg);
void handle_rally_point(mavlink_message_t *msg); void handle_rally_point(const mavlink_message_t &msg);
virtual void handle_mount_message(const mavlink_message_t *msg); virtual void handle_mount_message(const mavlink_message_t &msg);
void handle_fence_message(mavlink_message_t *msg); void handle_fence_message(const mavlink_message_t &msg);
void handle_param_value(mavlink_message_t *msg); void handle_param_value(const mavlink_message_t &msg);
void handle_radio_status(mavlink_message_t *msg, bool log_radio); void handle_radio_status(const mavlink_message_t &msg, bool log_radio);
void handle_serial_control(const mavlink_message_t *msg); void handle_serial_control(const mavlink_message_t &msg);
void handle_vision_position_delta(mavlink_message_t *msg); void handle_vision_position_delta(const mavlink_message_t &msg);
void handle_common_message(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_set_gps_global_origin(const mavlink_message_t &msg);
void handle_setup_signing(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; } virtual bool should_zero_rc_outputs_on_reboot() const { return false; }
MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet); 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); MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_flight_termination(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); MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet);
virtual void send_banner(); virtual void send_banner();
void handle_device_op_read(mavlink_message_t *msg); void handle_device_op_read(const mavlink_message_t &msg);
void handle_device_op_write(mavlink_message_t *msg); void handle_device_op_write(const mavlink_message_t &msg);
void send_timesync(); void send_timesync();
// returns the time a timesync message was most likely received: // returns the time a timesync message was most likely received:
uint64_t timesync_receive_timestamp_ns() const; uint64_t timesync_receive_timestamp_ns() const;
// returns a timestamp suitable for packing into the ts1 field of TIMESYNC: // returns a timestamp suitable for packing into the ts1 field of TIMESYNC:
uint64_t timesync_timestamp_ns() const; uint64_t timesync_timestamp_ns() const;
void handle_timesync(mavlink_message_t *msg); void handle_timesync(const mavlink_message_t &msg);
struct { struct {
int64_t sent_ts1; int64_t sent_ts1;
uint32_t last_sent_ms; uint32_t last_sent_ms;
const uint16_t interval_ms = 10000; const uint16_t interval_ms = 10000;
} _timesync_request; } _timesync_request;
void handle_statustext(mavlink_message_t *msg); void handle_statustext(const mavlink_message_t &msg);
bool telemetry_delayed() const; bool telemetry_delayed() const;
virtual uint32_t telem_delay() const = 0; 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_preflight_can(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_battery_reset(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); 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); virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_mag_cal(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_get_home_position(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_fence_enable(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 // vehicle-overridable message send function
virtual bool try_send_message(enum ap_message id); 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_compass_message(enum ap_message id);
bool try_send_mission_message(enum ap_message id); bool try_send_mission_message(enum ap_message id);
void send_hwstatus(); 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: // these two methods are called after current_loc is updated:
virtual int32_t global_position_int_alt() const; 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); 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); 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 bool handle_guided_request(AP_Mission::Mission_Command &cmd) = 0;
virtual void handle_change_alt_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_vicon_position_estimate(const mavlink_message_t &msg);
void handle_vision_position_estimate(mavlink_message_t *msg); void handle_vision_position_estimate(const mavlink_message_t &msg);
void handle_global_vision_position_estimate(mavlink_message_t *msg); void handle_global_vision_position_estimate(const mavlink_message_t &msg);
void handle_att_pos_mocap(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, void handle_common_vision_position_estimate_data(const uint64_t usec,
const float x, const float x,
const float y, const float y,
@ -856,7 +856,7 @@ private:
const float pitch, const float pitch,
const float yaw); 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 correct an offboard timestamp in microseconds to a local time

168
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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: // 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 // decode
mavlink_mission_request_list_t packet; 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); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
if (prot == nullptr) { if (prot == nullptr) {
mavlink_msg_mission_ack_send(chan, mavlink_msg_mission_ack_send(chan,
msg->sysid, msg.sysid,
msg->compid, msg.compid,
MAV_MISSION_UNSUPPORTED, MAV_MISSION_UNSUPPORTED,
packet.mission_type); packet.mission_type);
return; return;
} }
prot->handle_mission_request_list(*this, packet, *msg); prot->handle_mission_request_list(*this, packet, msg);
} }
void MissionItemProtocol::handle_mission_request_list( 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 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 // decode
mavlink_mission_request_int_t packet; 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); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
if (prot == nullptr) { if (prot == nullptr) {
return; 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, 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; 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 // decode
mavlink_mission_request_t packet; 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); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
if (prot == nullptr) { if (prot == nullptr) {
return; 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) 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 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 // decode
mavlink_mission_set_current_t packet; 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 // set current command
if (mission.set_current_cmd(packet.seq)) { 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 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 // decode
mavlink_mission_count_t packet; 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); MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
if (prot == nullptr) { if (prot == nullptr) {
mavlink_msg_mission_ack_send(chan, mavlink_msg_mission_ack_send(chan,
msg->sysid, msg.sysid,
msg->compid, msg.compid,
MAV_MISSION_UNSUPPORTED, MAV_MISSION_UNSUPPORTED,
packet.mission_type); packet.mission_type);
return; return;
} }
prot->handle_mission_count(*this, packet, *msg); prot->handle_mission_count(*this, packet, msg);
} }
void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link, 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 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 // decode
mavlink_mission_clear_all_t packet; 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; const MAV_MISSION_TYPE mission_type = (MAV_MISSION_TYPE)packet.mission_type;
MissionItemProtocol *prot = gcs().get_prot_for_mission_type(mission_type); MissionItemProtocol *prot = gcs().get_prot_for_mission_type(mission_type);
if (prot == nullptr) { if (prot == nullptr) {
send_mission_ack(*msg, mission_type, MAV_MISSION_UNSUPPORTED); send_mission_ack(msg, mission_type, MAV_MISSION_UNSUPPORTED);
return; return;
} }
prot->handle_mission_clear_all(*this, *msg); prot->handle_mission_clear_all(*this, msg);
} }
bool MissionItemProtocol_Waypoints::clear_all_items() bool MissionItemProtocol_Waypoints::clear_all_items()
@ -857,18 +857,18 @@ bool GCS_MAVLINK::requesting_mission_items() const
return false; 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 // decode
mavlink_mission_write_partial_list_t packet; 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); MissionItemProtocol *use_prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
if (use_prot == nullptr) { 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; 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, 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 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(); AP_Mount *mount = AP::mount();
if (mount == nullptr) { 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 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(); AP_Mount *mount = AP::mount();
if (mount == nullptr) { if (mount == nullptr) {
@ -927,10 +927,10 @@ void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *fmt, ...) const
va_end(arg_list); 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_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 // record if the GCS has been receiving radio messages from
// the aircraft // the aircraft
@ -971,21 +971,21 @@ void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, bool log_radio)
handle an incoming mission item handle an incoming mission item
return true if this is the last mission item, otherwise false 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 // TODO: rename packet to mission_item_int
mavlink_mission_item_int_t packet; 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_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); MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, packet);
if (ret != MAV_MISSION_ACCEPTED) { if (ret != MAV_MISSION_ACCEPTED) {
const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type; const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type;
send_mission_ack(*msg, type, ret); send_mission_ack(msg, type, ret);
return; return;
} }
} else { } else {
mavlink_msg_mission_item_int_decode(msg, &packet); mavlink_msg_mission_item_int_decode(&msg, &packet);
} }
const uint8_t current = packet.current; const uint8_t current = packet.current;
const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)packet.mission_type; 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); MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(packet, cmd);
if (result != MAV_MISSION_ACCEPTED) { if (result != MAV_MISSION_ACCEPTED) {
//decode failed //decode failed
send_mission_ack(*msg, MAV_MISSION_TYPE_MISSION, result); send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
return; return;
} }
// guided or change-alt // guided or change-alt
@ -1012,23 +1012,23 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t *msg)
// verify we recevied the command // verify we recevied the command
result = MAV_MISSION_ACCEPTED; result = MAV_MISSION_ACCEPTED;
} }
send_mission_ack(*msg, MAV_MISSION_TYPE_MISSION, result); send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
return; return;
} }
// not a guided-mode reqest // not a guided-mode reqest
MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type); MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type);
if (prot == nullptr) { if (prot == nullptr) {
send_mission_ack(*msg, type, MAV_MISSION_UNSUPPORTED); send_mission_ack(msg, type, MAV_MISSION_UNSUPPORTED);
return; return;
} }
if (!prot->receiving) { if (!prot->receiving) {
send_mission_ack(*msg, type, MAV_MISSION_ERROR); send_mission_ack(msg, type, MAV_MISSION_ERROR);
return; 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) 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, 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 // we exclude radio packets because we historically used this to
// make it possible to use the CLI over the radio // 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; 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 // the routing code has indicated we should not handle this packet locally
return; 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 // e.g. enforce-sysid says we shouldn't look at this packet
return; return;
} }
handleMessage(&msg); handleMessage(msg);
} }
void void
@ -2340,10 +2340,10 @@ void GCS_MAVLINK::send_battery2()
/* /*
handle a SET_MODE MAVLink message 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_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 MAV_MODE _base_mode = (MAV_MODE)packet.base_mode;
const uint32_t _custom_mode = packet.custom_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 return a timesync request
Sends back ts1 as received, and tc1 is the local timestamp in usec 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 // decode incoming timesync message
mavlink_timesync_t tsync; mavlink_timesync_t tsync;
mavlink_msg_timesync_decode(msg, &tsync); mavlink_msg_timesync_decode(&msg, &tsync);
if (tsync.tc1 != 0) { if (tsync.tc1 != 0) {
// this is a response to a timesync request // this is a response to a timesync request
@ -2921,7 +2921,7 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
#if 0 #if 0
gcs().send_text(MAV_SEVERITY_INFO, gcs().send_text(MAV_SEVERITY_INFO,
"timesync response sysid=%u (latency=%fms)", "timesync response sysid=%u (latency=%fms)",
msg->sysid, msg.sysid,
round_trip_time_us*0.001f); round_trip_time_us*0.001f);
#endif #endif
AP_Logger *logger = AP_Logger::get_singleton(); AP_Logger *logger = AP_Logger::get_singleton();
@ -2933,7 +2933,7 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
"F-F", "F-F",
"QBQ", "QBQ",
AP_HAL::micros64(), AP_HAL::micros64(),
msg->sysid, msg.sysid,
round_trip_time_us 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(); AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) { if (logger == nullptr) {
@ -2981,18 +2981,18 @@ void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg)
} }
mavlink_statustext_t packet; 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 max_prefix_len = 20;
const uint8_t text_len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+max_prefix_len; const uint8_t text_len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+max_prefix_len;
char text[text_len] = { 'G','C','S',':'}; char text[text_len] = { 'G','C','S',':'};
uint8_t offset = strlen(text); uint8_t offset = strlen(text);
if (msg->sysid != sysid_my_gcs()) { if (msg.sysid != sysid_my_gcs()) {
offset = hal.util->snprintf(text, offset = hal.util->snprintf(text,
max_prefix_len, max_prefix_len,
"SRC=%u/%u:", "SRC=%u/%u:",
msg->sysid, msg.sysid,
msg->compid); msg.compid);
} }
memcpy(&text[offset], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); 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_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); 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_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 // sanity check location
if (!check_latlng(packet.latitude, packet.longitude)) { 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 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 #if HAL_RCINPUT_WITH_AP_RADIO
mavlink_data96_t m; mavlink_data96_t m;
mavlink_msg_data96_decode(msg, &m); mavlink_msg_data96_decode(&msg, &m);
switch (m.type) { switch (m.type) {
case 42: case 42:
case 43: { case 43: {
@ -3122,7 +3122,7 @@ void GCS_MAVLINK::handle_data_packet(mavlink_message_t *msg)
#endif #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(); AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) { if (visual_odom == nullptr) {
@ -3131,28 +3131,28 @@ void GCS_MAVLINK::handle_vision_position_delta(mavlink_message_t *msg)
visual_odom->handle_msg(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_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, 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)); 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_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, 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)); 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_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, 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)); 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)); (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_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? // sensor assumed to be at 0,0,0 body-frame; need parameters for this?
const Vector3f sensor_offset = {}; 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); 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(); AP_AccelCal *accelcal = AP::ins().get_acal();
if (accelcal != nullptr) { 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 // allow override of RC channel values for HIL or for complete GCS
// control of switch position and RC PWM values. // 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 return; // Only accept control from our gcs
} }
const uint32_t tnow = AP_HAL::millis(); const uint32_t tnow = AP_HAL::millis();
mavlink_rc_channels_override_t packet; 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[] = { const uint16_t override_data[] = {
packet.chan1_raw, 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 // allow override of RC channel values for HIL or for complete GCS
// control of switch position and RC PWM values. // 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(); OpticalFlow *optflow = AP::opticalflow();
if (optflow == nullptr) { 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 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: { case MAVLINK_MSG_ID_COMMAND_ACK: {
handle_command_ack(msg); handle_command_ack(msg);
break; 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(); AP_Mission *_mission = AP::mission();
if (_mission == nullptr) { if (_mission == nullptr) {
return; return;
} }
switch (msg->msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38 case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
{ {
handle_mission_write_partial_list(msg); 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(); send_autopilot_version();
} }
@ -4034,11 +4034,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
return result; 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 // decode packet
mavlink_command_long_t 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; 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; 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 // decode packet
mavlink_command_int_t 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; 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 return true if we will accept this packet. Used to implement SYSID_ENFORCE
*/ */
bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, 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) { if (msg.sysid == mavlink_system.sysid) {
// accept packets from our own components // accept packets from our own components

8
libraries/GCS_MAVLink/GCS_DeviceOp.cpp
View File

@ -26,10 +26,10 @@ extern const AP_HAL::HAL& hal;
/* /*
handle DEVICE_OP_READ message 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_device_op_read_t packet;
mavlink_msg_device_op_read_decode(msg, &packet); mavlink_msg_device_op_read_decode(&msg, &packet);
AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr; AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
uint8_t retcode = 0; uint8_t retcode = 0;
uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {}; uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {};
@ -78,10 +78,10 @@ fail:
/* /*
handle DEVICE_OP_WRITE message 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_device_op_write_t packet;
mavlink_msg_device_op_write_decode(msg, &packet); mavlink_msg_device_op_write_decode(&msg, &packet);
AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr; AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
uint8_t retcode = 0; uint8_t retcode = 0;

2
libraries/GCS_MAVLink/GCS_Dummy.h
View File

@ -24,7 +24,7 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {};
class GCS_MAVLINK_Dummy : public GCS_MAVLINK class GCS_MAVLINK_Dummy : public GCS_MAVLINK
{ {
uint32_t telem_delay() const override { return 0; } 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 try_send_message(enum ap_message id) override { return true; }
bool handle_guided_request(AP_Mission::Mission_Command &cmd) 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 {} void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override {}

4
libraries/GCS_MAVLink/GCS_Fence.cpp
View File

@ -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(); AC_Fence *fence = AP::fence();
if (fence == nullptr) { if (fence == nullptr) {
return; return;
} }
// send or receive fence points with GCS // send or receive fence points with GCS
switch (msg->msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_FENCE_POINT: case MAVLINK_MSG_ID_FENCE_POINT:
case MAVLINK_MSG_ID_FENCE_FETCH_POINT: case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
fence->handle_msg(*this, msg); fence->handle_msg(*this, msg);

20
libraries/GCS_MAVLink/GCS_Param.cpp
View File

@ -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 save==false so we don't want the save to happen when the user connects the
ground station. 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_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 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()) { if (!params_ready()) {
return; return;
} }
mavlink_param_request_list_t packet; 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 // requesting parameters is a convenient way to get extra information
send_banner(); 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 _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) { if (param_requests.space() == 0) {
// we can't process this right now, drop it // 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_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 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_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet); mavlink_msg_param_set_decode(&msg, &packet);
enum ap_var_type var_type; enum ap_var_type var_type;
// set parameter // set parameter
@ -417,9 +417,9 @@ uint8_t GCS_MAVLINK::send_parameter_async_replies()
return async_replies_sent_count; 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: case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
handle_param_request_list(msg); handle_param_request_list(msg);
break; break;

14
libraries/GCS_MAVLink/GCS_Rally.cpp
View File

@ -19,7 +19,7 @@
#include <AP_Rally/AP_Rally.h> #include <AP_Rally/AP_Rally.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
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(); AP_Rally *r = AP::rally();
if (r == nullptr) { if (r == nullptr) {
@ -27,7 +27,7 @@ void GCS_MAVLINK::handle_rally_point(mavlink_message_t *msg)
} }
mavlink_rally_point_t packet; 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() || if (packet.idx >= r->get_rally_total() ||
packet.idx >= r->get_rally_max()) { 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(); AP_Rally *r = AP::rally();
if (r == nullptr) { 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_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()) { if (packet.idx > r->get_rally_total()) {
send_text(MAV_SEVERITY_WARNING, "Bad rally point ID"); 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; 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, r->get_rally_total(), rally_point.lat, rally_point.lng,
rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.alt, rally_point.break_alt, rally_point.land_dir,
rally_point.flags); 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: case MAVLINK_MSG_ID_RALLY_POINT:
handle_rally_point(msg); handle_rally_point(msg);
break; break;

4
libraries/GCS_MAVLink/GCS_Signing.cpp
View File

@ -63,7 +63,7 @@ bool GCS_MAVLINK::signing_key_load(struct SigningKey &key)
/* /*
handle a setup_signing message 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 / // setting up signing key when armed generally not useful /
// possibly not a good idea // possibly not a good idea
@ -74,7 +74,7 @@ void GCS_MAVLINK::handle_setup_signing(const mavlink_message_t *msg)
// decode // decode
mavlink_setup_signing_t packet; mavlink_setup_signing_t packet;
mavlink_msg_setup_signing_decode(msg, &packet); mavlink_msg_setup_signing_decode(&msg, &packet);
struct SigningKey key; struct SigningKey key;
key.magic = SIGNING_KEY_MAGIC; key.magic = SIGNING_KEY_MAGIC;

4
libraries/GCS_MAVLink/GCS_serial_control.cpp
View File

@ -28,10 +28,10 @@ extern const AP_HAL::HAL& hal;
/** /**
handle a SERIAL_CONTROL message 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_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::UARTDriver *port = nullptr;
AP_HAL::BetterStream *stream = nullptr; AP_HAL::BetterStream *stream = nullptr;

76
libraries/GCS_MAVLink/MAVLink_routing.cpp
View File

@ -88,31 +88,31 @@ detect a reset of the flight controller, which implies a reset of its
routing table. 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 // handle the case of loopback of our own messages, due to
// incorrect serial configuration. // incorrect serial configuration.
if (msg->sysid == mavlink_system.sysid && if (msg.sysid == mavlink_system.sysid &&
msg->compid == mavlink_system.compid) { msg.compid == mavlink_system.compid) {
return true; return true;
} }
// learn new routes // learn new routes
learn_route(in_channel, msg); learn_route(in_channel, msg);
if (msg->msgid == MAVLINK_MSG_ID_RADIO || if (msg.msgid == MAVLINK_MSG_ID_RADIO ||
msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
// don't forward RADIO packets // don't forward RADIO packets
return true; return true;
} }
if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) { if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
// heartbeat needs special handling // heartbeat needs special handling
handle_heartbeat(in_channel, msg); handle_heartbeat(in_channel, msg);
return true; 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 // ADSB packets are not forwarded, they have their own stream rate
return true; 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 (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)) { GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
#if ROUTING_DEBUG #if ROUTING_DEBUG
::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n", ::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n",
msg->msgid, msg.msgid,
(unsigned)in_channel, (unsigned)in_channel,
(unsigned)routes[i].channel, (unsigned)routes[i].channel,
(int)target_system, (int)target_system,
(int)target_component); (int)target_component);
#endif #endif
_mavlink_resend_uart(routes[i].channel, msg); _mavlink_resend_uart(routes[i].channel, &msg);
} }
sent_to_chan[routes[i].channel] = true; sent_to_chan[routes[i].channel] = true;
forwarded = 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 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]; bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS];
memset(sent_to_chan, 0, sizeof(sent_to_chan)); 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 // check learned routes
for (uint8_t i=0; i<num_routes; i++) { for (uint8_t i=0; i<num_routes; i++) {
if ((routes[i].sysid == mavlink_system.sysid) && !sent_to_chan[routes[i].channel]) { if ((routes[i].sysid == mavlink_system.sysid) && !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)) { GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
#if ROUTING_DEBUG #if ROUTING_DEBUG
::printf("send msg %u on chan %u sysid=%u compid=%u\n", ::printf("send msg %u on chan %u sysid=%u compid=%u\n",
msg->msgid, msg.msgid,
(unsigned)routes[i].channel, (unsigned)routes[i].channel,
(unsigned)routes[i].sysid, (unsigned)routes[i].sysid,
(unsigned)routes[i].compid); (unsigned)routes[i].compid);
#endif #endif
_mavlink_resend_uart(routes[i].channel, msg); _mavlink_resend_uart(routes[i].channel, &msg);
sent_to_chan[routes[i].channel] = true; 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 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; uint8_t i;
if (msg->sysid == 0 || if (msg.sysid == 0 ||
(msg->sysid == mavlink_system.sysid && (msg.sysid == mavlink_system.sysid &&
msg->compid == mavlink_system.compid)) { msg.compid == mavlink_system.compid)) {
return; return;
} }
for (i=0; i<num_routes; i++) { for (i=0; i<num_routes; i++) {
if (routes[i].sysid == msg->sysid && if (routes[i].sysid == msg.sysid &&
routes[i].compid == msg->compid && routes[i].compid == msg.compid &&
routes[i].channel == in_channel) { routes[i].channel == in_channel) {
if (routes[i].mavtype == 0 && msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) { if (routes[i].mavtype == 0 && msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
routes[i].mavtype = mavlink_msg_heartbeat_get_type(msg); routes[i].mavtype = mavlink_msg_heartbeat_get_type(&msg);
} }
break; break;
} }
} }
if (i == num_routes && i<MAVLINK_MAX_ROUTES) { if (i == num_routes && i<MAVLINK_MAX_ROUTES) {
routes[i].sysid = msg->sysid; routes[i].sysid = msg.sysid;
routes[i].compid = msg->compid; routes[i].compid = msg.compid;
routes[i].channel = in_channel; routes[i].channel = in_channel;
if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) { if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
routes[i].mavtype = mavlink_msg_heartbeat_get_type(msg); routes[i].mavtype = mavlink_msg_heartbeat_get_type(&msg);
} }
num_routes++; num_routes++;
#if ROUTING_DEBUG #if ROUTING_DEBUG
::printf("learned route %u %u via %u\n", ::printf("learned route %u %u via %u\n",
(unsigned)msg->sysid, (unsigned)msg.sysid,
(unsigned)msg->compid, (unsigned)msg.compid,
(unsigned)in_channel); (unsigned)in_channel);
#endif #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 propagation heartbeat messages need to be forwarded on all channels
except channels where the sysid/compid of the heartbeat could come from 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(); 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 // mask out channels that are known sources for this sysid/compid
for (uint8_t i=0; i<num_routes; i++) { for (uint8_t i=0; i<num_routes; i++) {
if (routes[i].sysid == msg->sysid && 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))); 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<MAVLINK_COMM_NUM_BUFFERS; i++) { for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if (mask & (1U<<i)) { if (mask & (1U<<i)) {
mavlink_channel_t channel = (mavlink_channel_t)(MAVLINK_COMM_0 + i); mavlink_channel_t channel = (mavlink_channel_t)(MAVLINK_COMM_0 + i);
if (comm_get_txspace(channel) >= ((uint16_t)msg->len) + if (comm_get_txspace(channel) >= ((uint16_t)msg.len) +
GCS_MAVLINK::packet_overhead_chan(channel)) { GCS_MAVLINK::packet_overhead_chan(channel)) {
#if ROUTING_DEBUG #if ROUTING_DEBUG
::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n", ::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n",
(unsigned)in_channel, (unsigned)in_channel,
(unsigned)channel, (unsigned)channel,
(unsigned)msg->sysid, (unsigned)msg.sysid,
(unsigned)msg->compid); (unsigned)msg.compid);
#endif #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 that the caller can set them to -1 and know when a sysid or compid
target is found in the message 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) { if (msg_entry == nullptr) {
return; return;
} }
if (msg_entry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { 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) { 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);
} }
} }

10
libraries/GCS_MAVLink/MAVLink_routing.h
View File

@ -27,13 +27,13 @@ public:
This returns true if the message should be processed locally 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 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 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 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; uint8_t no_route_mask;
// learn new routes // 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 // 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 // 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);
}; };

12
libraries/GCS_MAVLink/examples/routing/routing.cpp
View File

@ -39,7 +39,7 @@ void loop(void)
mavlink_msg_heartbeat_encode(3, 1, &msg, &heartbeat); 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"); hal.console->printf("heartbeat should be processed locally\n");
err_count++; err_count++;
} }
@ -47,7 +47,7 @@ void loop(void)
// incoming non-targetted message // incoming non-targetted message
mavlink_attitude_t attitude = {0}; mavlink_attitude_t attitude = {0};
mavlink_msg_attitude_encode(3, 1, &msg, &attitude); 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"); hal.console->printf("attitude should be processed locally\n");
err_count++; err_count++;
} }
@ -57,7 +57,7 @@ void loop(void)
param_set.target_system = mavlink_system.sysid+1; param_set.target_system = mavlink_system.sysid+1;
param_set.target_component = mavlink_system.compid; param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_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"); hal.console->printf("param set 1 should not be processed locally\n");
err_count++; err_count++;
} }
@ -66,7 +66,7 @@ void loop(void)
param_set.target_system = mavlink_system.sysid; param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid; param_set.target_component = mavlink_system.compid;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_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"); hal.console->printf("param set 2 should be processed locally\n");
err_count++; err_count++;
} }
@ -76,7 +76,7 @@ void loop(void)
param_set.target_system = mavlink_system.sysid; param_set.target_system = mavlink_system.sysid;
param_set.target_component = mavlink_system.compid+1; param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_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"); hal.console->printf("param set 3 should be processed locally\n");
err_count++; err_count++;
} }
@ -85,7 +85,7 @@ void loop(void)
param_set.target_system = 0; param_set.target_system = 0;
param_set.target_component = mavlink_system.compid+1; param_set.target_component = mavlink_system.compid+1;
mavlink_msg_param_set_encode(3, 1, &msg, &param_set); mavlink_msg_param_set_encode(3, 1, &msg, &param_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"); hal.console->printf("param set 4 should be processed locally\n");
err_count++; err_count++;
} }