diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 88197fada0..01ac2a9f6a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -701,7 +701,7 @@ MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlin } void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status, - mavlink_message_t &msg) + const mavlink_message_t &msg) { plane.avoidance_adsb.handle_msg(msg); GCS_MAVLINK::packetReceived(status, msg); @@ -1007,15 +1007,15 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } } -void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) +void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) { - switch (msg->msgid) { + switch (msg.msgid) { #if GEOFENCE_ENABLED == ENABLED // receive a fence point from GCS and store in EEPROM case MAVLINK_MSG_ID_FENCE_POINT: { mavlink_fence_point_t packet; - mavlink_msg_fence_point_decode(msg, &packet); + mavlink_msg_fence_point_decode(&msg, &packet); if (plane.g.fence_action != FENCE_ACTION_NONE) { send_text(MAV_SEVERITY_WARNING,"Fencing must be disabled"); } else if (packet.count != plane.g.fence_total) { @@ -1031,12 +1031,12 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // send a fence point to GCS case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { mavlink_fence_fetch_point_t packet; - mavlink_msg_fence_fetch_point_decode(msg, &packet); + mavlink_msg_fence_fetch_point_decode(&msg, &packet); if (packet.idx >= plane.g.fence_total) { send_text(MAV_SEVERITY_WARNING,"Bad fence point"); } else { Vector2l point = plane.get_fence_point_with_index(packet.idx); - mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, plane.g.fence_total, + mavlink_msg_fence_point_send(chan, msg.sysid, msg.compid, packet.idx, plane.g.fence_total, point.x*1.0e-7f, point.y*1.0e-7f); } break; @@ -1046,12 +1046,12 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_MANUAL_CONTROL: { - if (msg->sysid != plane.g.sysid_my_gcs) { + if (msg.sysid != plane.g.sysid_my_gcs) { break; // only accept control from our gcs } mavlink_manual_control_t packet; - mavlink_msg_manual_control_decode(msg, &packet); + mavlink_msg_manual_control_decode(&msg, &packet); if (packet.target != plane.g.sysid_this_mav) { break; // only accept messages aimed at us @@ -1073,7 +1073,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) { // We keep track of the last time we received a heartbeat from // our GCS for failsafe purposes - if (msg->sysid != plane.g.sysid_my_gcs) break; + if (msg.sysid != plane.g.sysid_my_gcs) break; plane.failsafe.last_heartbeat_ms = AP_HAL::millis(); break; } @@ -1086,7 +1086,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) } mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(msg, &packet); + mavlink_msg_hil_state_decode(&msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { @@ -1168,7 +1168,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) } mavlink_set_attitude_target_t att_target; - mavlink_msg_set_attitude_target_decode(msg, &att_target); + mavlink_msg_set_attitude_target_decode(&msg, &att_target); // Mappings: If any of these bits are set, the corresponding input should be ignored. // NOTE, when parsing the bits we invert them for easier interpretation but transport has them inverted @@ -1229,7 +1229,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_SET_HOME_POSITION: { mavlink_set_home_position_t packet; - mavlink_msg_set_home_position_decode(msg, &packet); + mavlink_msg_set_home_position_decode(&msg, &packet); Location new_home_loc {}; new_home_loc.lat = packet.latitude; new_home_loc.lng = packet.longitude; @@ -1245,7 +1245,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) { // decode packet mavlink_set_position_target_local_ned_t packet; - mavlink_msg_set_position_target_local_ned_decode(msg, &packet); + mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); // exit if vehicle is not in Guided mode if (plane.control_mode != &plane.mode_guided) { @@ -1279,7 +1279,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) } mavlink_set_position_target_global_int_t pos_target; - mavlink_msg_set_position_target_global_int_decode(msg, &pos_target); + mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target); // Unexpectedly, the mask is expecting "ones" for dimensions that should // be IGNORNED rather than INCLUDED. See mavlink documentation of the // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field. @@ -1332,7 +1332,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) } // end handle mavlink // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes -void GCS_MAVLINK_Plane::handle_rc_channels_override(const mavlink_message_t *msg) +void GCS_MAVLINK_Plane::handle_rc_channels_override(const mavlink_message_t &msg) { plane.failsafe.last_heartbeat_ms = AP_HAL::millis(); GCS_MAVLINK::handle_rc_channels_override(msg); @@ -1371,7 +1371,7 @@ void Plane::mavlink_delay_cb() logger.EnableWrites(true); } -void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg) +void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg) { plane.auto_state.next_wp_crosstrack = false; GCS_MAVLINK::handle_mission_set_current(mission, msg); diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 75c51202e0..5f799a2003 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -12,7 +12,7 @@ protected: uint32_t telem_delay() const override; - void handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg) override; + void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg) override; AP_AdvancedFailsafe *get_advanced_failsafe() const override; @@ -46,12 +46,12 @@ private: void send_pid_info(const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved); - void handleMessage(mavlink_message_t * msg) override; + void handleMessage(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; - void handle_rc_channels_override(const mavlink_message_t *msg) override; + void handle_rc_channels_override(const mavlink_message_t &msg) override; bool try_send_message(enum ap_message id) override; - void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override; + void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; MAV_MODE base_mode() const override; MAV_STATE system_status() const override;