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.mode_guided.reset_guided_hdg();
+        plane.mode_guided.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.mode_guided.reset_guided_spd();
+
             return MAV_RESULT_ACCEPTED;
         }
         return MAV_RESULT_FAILED;

ArduPlane/commands_logic.cpp

@@ -1306,3 +1306,4 @@ bool Plane::in_auto_mission_id(uint16_t command) const
 {
     return control_mode == &mode_auto && mission.get_current_nav_id() == command;
 }
+

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.mode_guided.reset_guided_hdg();
+    plane.mode_guided.reset_guided_alt();
+    plane.mode_guided.reset_guided_spd();
+
 #endif
 
 #if AP_CAMERA_ENABLED

ArduPlane/mode.h

@@ -9,6 +9,7 @@
 #include "quadplane.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Mission/AP_Mission.h>
+#include "config.h"
 
 class AC_PosControl;
 class AC_AttitudeControl_Multi;
@@ -141,6 +142,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
@@ -308,6 +313,12 @@ class ModeGuided : public Mode
 
     void update_target_altitude() override;
 
+#if OFFBOARD_GUIDED == ENABLED
+    void reset_guided_hdg();
+    void reset_guided_alt();
+    void reset_guided_spd();
+#endif
+
 protected:
 
     bool _enter() override;

ArduPlane/mode_guided.cpp

@@ -153,3 +153,24 @@ void ModeGuided::update_target_altitude()
         Mode::update_target_altitude();
     }
 }
+
+// Clear guided_state Heading values
+void ModeGuided::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 ModeGuided::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 ModeGuided::reset_guided_spd()
+{
+    plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
+}