Added re-initialization for guided_change_x commands

ArduPlane/GCS_Mavlink.cpp

@@ -859,6 +859,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
             plane.mode_guided.set_radius_and_direction(packet.param3, requested_position.loiter_ccw);
         }
 
+        // Need to re-initialize these, otherwise DO_REPOSITION will be overruled by GUIDED_CHANGE_HEADING and GUIDED_CHANGE_ALTITUDE if previously sent
+        plane.reset_guided_hdg();
+        plane.reset_guided_alt();
+
         return MAV_RESULT_ACCEPTED;
     }
     return MAV_RESULT_FAILED;
@@ -1101,6 +1105,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_comma
         }
 
         if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) {
+
+            // Need to re-initialize this, otherwise DO_CHANGE_SPEED will be overrided by any prior GUIDED_CHANGE_SPEED command while in GUIDED
+            plane.reset_guided_spd();
+
             return MAV_RESULT_ACCEPTED;
         }
         return MAV_RESULT_FAILED;

ArduPlane/Plane.h

@@ -954,6 +954,11 @@ class Plane : public AP_Vehicle {
     bool start_command_callback(const AP_Mission::Mission_Command &cmd);
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     float get_wp_radius() const;
+#if OFFBOARD_GUIDED == ENABLED
+    void reset_guided_hdg();
+    void reset_guided_alt();
+    void reset_guided_spd();
+#endif
 
     bool is_land_command(uint16_t cmd) const;
 

ArduPlane/commands_logic.cpp

@@ -1306,3 +1306,26 @@ bool Plane::in_auto_mission_id(uint16_t command) const
 {
     return control_mode == &mode_auto && mission.get_current_nav_id() == command;
 }
+
+
+// Clear guided_state Heading values
+void Plane::reset_guided_hdg()
+{
+    plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
+    plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
+}
+
+// Clear guided_state altitude values
+void Plane::reset_guided_alt()
+{
+    plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
+    plane.guided_state.target_alt_time_ms = 0;
+    plane.guided_state.last_target_alt = 0;
+}
+
+// Clear guided_state speed value
+void Plane::reset_guided_spd()
+{
+    plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
+}
+

ArduPlane/mode.cpp

@@ -55,12 +55,11 @@ bool Mode::enter()
     plane.guided_state.last_forced_throttle_ms = 0;
 
 #if OFFBOARD_GUIDED == ENABLED
-    plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
-    plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
-    plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
-    plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
-    plane.guided_state.target_alt_time_ms = 0;
-    plane.guided_state.last_target_alt = 0;
+
+    plane.reset_guided_hdg();
+    plane.reset_guided_alt();
+    plane.reset_guided_spd();
+
 #endif
 
 #if AP_CAMERA_ENABLED

ArduPlane/mode.h

@@ -141,6 +141,10 @@ class Mode
     // true if voltage correction should be applied to throttle
     virtual bool use_battery_compensation() const;
 
+    void reset_guided_hdg();
+    void reset_guided_alt();
+    void reset_guided_spd();
+
 protected:
 
     // subclasses override this to perform checks before entering the mode