From 3e5665735f66a929ff4d3ed0c34b2a5288de1f5f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 18 Jul 2017 18:55:22 +1000 Subject: [PATCH] Copter: move try_send_message mission handling up --- ArduCopter/Copter.h | 1 - ArduCopter/GCS_Mavlink.cpp | 20 -------------------- 2 files changed, 21 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index cb021e4b98..058894a38f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -708,7 +708,6 @@ private: void send_simstate(mavlink_channel_t chan); void send_hwstatus(mavlink_channel_t chan); void send_vfr_hud(mavlink_channel_t chan); - void send_current_waypoint(mavlink_channel_t chan); void send_rpm(mavlink_channel_t chan); void rpm_update(); void button_update(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9133617cda..72d3c5baf7 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -202,11 +202,6 @@ void NOINLINE Copter::send_vfr_hud(mavlink_channel_t chan) climb_rate / 100.0f); } -void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan) -{ - mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); -} - /* send RPM packet */ @@ -393,21 +388,11 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) send_sensor_offsets(copter.ins, copter.compass, copter.barometer); break; - case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(MISSION_CURRENT); - copter.send_current_waypoint(chan); - break; - case MSG_NEXT_PARAM: CHECK_PAYLOAD_SIZE(PARAM_VALUE); queued_param_send(); break; - case MSG_NEXT_WAYPOINT: - CHECK_PAYLOAD_SIZE(MISSION_REQUEST); - queued_waypoint_send(); - break; - case MSG_RANGEFINDER: #if RANGEFINDER_ENABLED == ENABLED CHECK_PAYLOAD_SIZE(RANGEFINDER); @@ -515,11 +500,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) send_vibration(copter.ins); break; - case MSG_MISSION_ITEM_REACHED: - CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED); - mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index); - break; - case MSG_ADSB_VEHICLE: CHECK_PAYLOAD_SIZE(ADSB_VEHICLE); copter.adsb.send_adsb_vehicle(chan);