Create and use pr/HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED

ArduCopter/GCS_Mavlink.cpp

@@ -613,6 +613,7 @@ MISSION_STATE GCS_MAVLINK_Copter::mission_state(const class AP_Mission &mission)
     return GCS_MAVLINK::mission_state(mission);
 }
 
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
 bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
 #if MODE_AUTO_ENABLED
@@ -621,6 +622,7 @@ bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd)
     return false;
 #endif
 }
+#endif  // HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
 
 void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,
                                         const mavlink_message_t &msg)

ArduCopter/GCS_Mavlink.h

@@ -75,7 +75,9 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
 
     void handle_message(const mavlink_message_t &msg) override;
     void handle_command_ack(const mavlink_message_t &msg) override;
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
+#endif
     bool try_send_message(enum ap_message id) override;
 
     void packetReceived(const mavlink_status_t &status,

ArduPlane/GCS_Mavlink.cpp

@@ -733,6 +733,7 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
     MAV_STREAM_TERMINATOR // must have this at end of stream_entries
 };
 
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
 /*
   handle a request to switch to guided mode. This happens via a
   callback from handle_mission_item()
@@ -741,6 +742,7 @@ bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
     return plane.control_mode->handle_guided_request(cmd.content.location);
 }
+#endif  // HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
 
 /*
   handle a request to change current WP altitude. This happens via a
@@ -757,7 +759,6 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c
     plane.reset_offset_altitude();
 }
 
-
 /*
   handle a LANDING_TARGET command. The timestamp has been jitter corrected
 */

ArduPlane/GCS_Mavlink.h

@@ -55,8 +55,14 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK
     void send_pid_info(const struct AP_PIDInfo *pid_info, const uint8_t axis, const float achieved);
 
     void handle_message(const mavlink_message_t &msg) override;
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
+#else
+    // this method can be folded into its remaining caller when
+    // guided-mode-requests are removed:
+    void handle_change_alt_request(AP_Mission::Mission_Command &cmd);
+#endif
     MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
     MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet);
     MAV_RESULT handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet);

ArduSub/GCS_Mavlink.cpp

@@ -452,10 +452,12 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
     MAV_STREAM_TERMINATOR // must have this at end of stream_entries
 };
 
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
 bool GCS_MAVLINK_Sub::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
     return sub.do_guided(cmd);
 }
+#endif
 
 MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
 {

ArduSub/GCS_Mavlink.h

@@ -40,7 +40,9 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK {
 private:
 
     void handle_message(const mavlink_message_t &msg) override;
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
+#endif
     bool try_send_message(enum ap_message id) override;
 
     bool send_info(void);

Rover/GCS_Mavlink.cpp

@@ -636,6 +636,7 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
     MAV_STREAM_TERMINATOR // must have this at end of stream_entries
 };
 
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
 bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
     if (!rover.control_mode->in_guided_mode()) {
@@ -646,6 +647,7 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
     // make any new wp uploaded instant (in case we are already in Guided mode)
     return rover.mode_guided.set_desired_location(cmd.content.location);
 }
+#endif  // HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
 
 MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 {

Rover/GCS_Mavlink.h

@@ -43,7 +43,9 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK
 private:
 
     void handle_message(const mavlink_message_t &msg) override;
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
+#endif
     bool try_send_message(enum ap_message id) override;
 
     void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override;

Tools/AP_Periph/GCS_MAVLink.h

@@ -29,7 +29,9 @@ class GCS_MAVLINK_Periph : public GCS_MAVLINK
 private:
 
     uint32_t telem_delay() const override { return 0; }
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override { return true; }
+#endif
     MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
     uint8_t sysid_my_gcs() const override;
 

Tools/scripts/build_options.py

@@ -431,6 +431,9 @@ def config_option(self):
     Feature('Networking', 'CAN MCAST', 'AP_NETWORKING_CAN_MCAST_ENABLED', 'Enable CAN multicast bridge', 0, None),
 
     Feature('DroneCAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None),
+
+    Feature('MAVLink', 'GUIDED_MISSION_REQUESTS', 'HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED', 'Enable changing position and location with mission requests in guided mode', 0, None),  # noqa:E501
+
 ]
 
 BUILD_OPTIONS.sort(key=lambda x: (x.category + x.label))

Tools/scripts/extract_features.py

@@ -257,6 +257,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED', 'Plane::handle_external_hagl'),
             ('AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED', 'AP_Camera::send_video_stream_information'),
 
+            ('HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED', 'GCS_MAVLINK::handle_mission_item_guided_mode_request'),
+
             ('AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'AP_DroneCAN::SRV_send_himark'),
             ('AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'AP_DroneCAN::hobbywing_ESC_update'),
             ('COMPASS_CAL_ENABLED', 'CompassCalibrator::stop'),

libraries/GCS_MAVLink/GCS.h

@@ -1038,8 +1038,10 @@ class GCS_MAVLINK
 
     void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const;
 
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
     virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) { return false; };
     virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) {};
+#endif
     void handle_common_mission_message(const mavlink_message_t &msg);
 
     virtual void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) {};
@@ -1126,6 +1128,12 @@ class GCS_MAVLINK
     // true if we should NOT do MAVLink on this port (usually because
     // someone's doing SERIAL_CONTROL over mavlink)
     bool _locked;
+
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
+    // handle a mission item int uploaded with current==2 or
+    // current==3, meaning go somewhere when in guided mode:
+    void handle_mission_item_guided_mode_request(const mavlink_message_t &msg, const mavlink_mission_item_int_t &mission_item_int);
+#endif
 };
 
 /// @class GCS

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -912,6 +912,36 @@ void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg)
 #endif
 }
 
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
+void GCS_MAVLINK::handle_mission_item_guided_mode_request(const mavlink_message_t &msg, const mavlink_mission_item_int_t &mission_item_int)
+{
+        const uint8_t current = mission_item_int.current;
+
+        struct AP_Mission::Mission_Command cmd = {};
+        MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
+        if (result != MAV_MISSION_ACCEPTED) {
+            //decode failed
+            send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
+            return;
+        }
+        // guided or change-alt
+        if (current == 2) {
+            // current = 2 is a flag to tell us this is a "guided mode"
+            // waypoint and not for the mission
+            result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
+                      : MAV_MISSION_ERROR) ;
+        } else if (current == 3) {
+            //current = 3 is a flag to tell us this is a alt change only
+            // add home alt if needed
+            handle_change_alt_request(cmd);
+
+            // verify we received the command
+            result = MAV_MISSION_ACCEPTED;
+        }
+        send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
+}
+#endif  // AP_MISSION_ENABLED
+
 void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
 {
     mavlink_mission_item_int_t mission_item_int;
@@ -941,32 +971,11 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
     }
     const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type;
 
-#if AP_MISSION_ENABLED
+#if HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
     const uint8_t current = mission_item_int.current;
 
     if (type == MAV_MISSION_TYPE_MISSION && (current == 2 || current == 3)) {
-        struct AP_Mission::Mission_Command cmd = {};
-        MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
-        if (result != MAV_MISSION_ACCEPTED) {
-            //decode failed
-            send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
-            return;
-        }
-        // guided or change-alt
-        if (current == 2) {
-            // current = 2 is a flag to tell us this is a "guided mode"
-            // waypoint and not for the mission
-            result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
-                      : MAV_MISSION_ERROR) ;
-        } else if (current == 3) {
-            //current = 3 is a flag to tell us this is a alt change only
-            // add home alt if needed
-            handle_change_alt_request(cmd);
-
-            // verify we received the command
-            result = MAV_MISSION_ACCEPTED;
-        }
-        send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
+        handle_mission_item_guided_mode_request(msg, mission_item_int);
         return;
     }
 #endif

libraries/GCS_MAVLink/GCS_config.h

@@ -139,3 +139,7 @@
 #ifndef AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED
 #define AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED HAL_GCS_ENABLED
 #endif
+
+#ifndef HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED
+#define HAL_GCS_GUIDED_MISSION_REQUESTS_ENABLED AP_MISSION_ENABLED
+#endif