Plane: fix DO_CHANGE_SPEED airspeed to impact only AUTO and GUIDED modes

ArduPlane/AP_Arming.cpp

@@ -248,9 +248,6 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
     //only log if disarming was successful
     change_arm_state();
 
-    // reload target airspeed which could have been modified by a mission
-    plane.aparm.airspeed_cruise_cm.load();
-
 #if QAUTOTUNE_ENABLED
     //save qautotune gains if enabled and success
     if (plane.control_mode == &plane.mode_qautotune) {

ArduPlane/Plane.h

@@ -376,6 +376,7 @@ private:
     // The calculated airspeed to use in FBW-B.  Also used in higher modes for insuring min ground speed is met.
     // Also used for flap deployment criteria.  Centimeters per second.
     int32_t target_airspeed_cm;
+    int32_t new_airspeed_cm = -1;  //temp variable for AUTO and GUIDED mode speed changes
 
     // The difference between current and desired airspeed.  Used in the pitch controller.  Meters per second.
     float airspeed_error;

ArduPlane/commands_logic.cpp

@@ -880,7 +880,7 @@ bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
     {
     case 0:             // Airspeed
         if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && (cmd.content.speed.target_ms <= aparm.airspeed_max.get()))  {
-            aparm.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100);
+           new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes
             gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms);
             return true;
         }

ArduPlane/mode.cpp

@@ -64,6 +64,9 @@ bool Mode::enter()
     plane.auto_state.vtol_mode = false;
     plane.auto_state.vtol_loiter = false;
 
+    // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
+    plane.new_airspeed_cm = -1;
+
     bool enter_result = _enter();
 
     if (enter_result) {

ArduPlane/mode_guided.cpp

@@ -10,7 +10,6 @@ bool ModeGuided::_enter()
     */
     plane.guided_WP_loc = plane.current_loc;
     plane.set_guided_WP();
-
     return true;
 }
 
@@ -30,4 +29,3 @@ void ModeGuided::navigate()
     // Zero indicates to use WP_LOITER_RAD
     plane.update_loiter(0);
 }
-

ArduPlane/navigation.cpp

@@ -166,7 +166,7 @@ void Plane::calc_airspeed_errors()
                                   get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
         }
 #if OFFBOARD_GUIDED == ENABLED
-    } else if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm)) {
+    } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm >  0.0) { // if offbd guided speed change cmd not set, then this section is skipped
         // offboard airspeed demanded
         uint32_t now = AP_HAL::millis();
         float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms);
@@ -182,11 +182,14 @@ void Plane::calc_airspeed_errors()
         }
 
 #endif // OFFBOARD_GUIDED == ENABLED
+
     } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
         // Landing airspeed target
         target_airspeed_cm = landing.get_target_airspeed_cm();
-    } else if ((control_mode == &mode_auto) &&
-               (quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
+    } else if (control_mode == &mode_guided && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
+                       target_airspeed_cm = new_airspeed_cm;
+    } else if (control_mode == &mode_auto) {
+        if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
 			   ((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
 			     (vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
                      const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
@@ -197,7 +200,13 @@ void Plane::calc_airspeed_errors()
                          target_airspeed_cm = aparm.airspeed_cruise_cm;
                      }
         } else {
-        // Normal airspeed target
+            // normal AUTO mode and new_airspeed variable was set by DO_CHANGE_SPEED command while in AUTO mode
+            if (new_airspeed_cm > 0) {
+                target_airspeed_cm = new_airspeed_cm;
+           }
+        }
+    } else {
+        // Normal airspeed target for all other cases
         target_airspeed_cm = aparm.airspeed_cruise_cm;
     }
 
@@ -390,4 +399,3 @@ bool Plane::reached_loiter_target(void)
     }
     return nav_controller->reached_loiter_target();
 }
-    

ArduPlane/quadplane.cpp

@@ -3005,8 +3005,6 @@ bool QuadPlane::check_land_complete(void)
     if (land_detector(4000)) {
         poscontrol.state = QPOS_LAND_COMPLETE;
         gcs().send_text(MAV_SEVERITY_INFO,"Land complete");
-        // reload target airspeed which could have been modified by the mission
-        plane.aparm.airspeed_cruise_cm.load();
         if (plane.control_mode != &plane.mode_auto ||
             !plane.mission.continue_after_land()) {
             // disarm on land unless we have MIS_OPTIONS setup to

Tools/autotest/arduplane.py

@@ -2438,7 +2438,7 @@ class AutoTestPlane(AutoTest):
                     50    # alt
                 )
                 self.delay_sim_time(5)
-                new_target_groundspeed = m.groundspeed + 5
+                new_target_groundspeed = m.groundspeed + 10
                 self.run_cmd(
                     mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
                     1, # groundspeed
@@ -2552,7 +2552,7 @@ class AutoTestPlane(AutoTest):
         self.wait_ready_to_arm()
         self.takeoff(alt=50)
         self.change_mode("CRUISE")
-        self.wait_distance(150, accuracy=20)
+        self.wait_distance(90, accuracy=15)
 
         self.progress("Enable fence and initiate fence action")
         self.do_fence_enable()
@@ -2633,8 +2633,8 @@ class AutoTestPlane(AutoTest):
         self.progress("Return loc: (%s)" % str(ret_loc))
 
         # Wait for guided return to vehicle calculated fence return location
-        self.wait_distance_to_location(ret_loc, 105, 115)
-        self.wait_circling_point_with_radius(ret_loc, want_radius)
+        self.wait_distance_to_location(ret_loc, 90, 110)
+        self.wait_circling_point_with_radius(ret_loc, 92)
 
         self.progress("Test complete, disable fence and come home")
         self.do_fence_disable()