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

@@ -311,7 +311,7 @@ private:
 
     // last time we ran roll/pitch stabilization
     uint32_t last_stabilize_ms;
-    
+
     // Failsafe
     struct {
         // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
@@ -336,10 +336,10 @@ private:
 
         // the time when the last HEARTBEAT message arrived from a GCS
         uint32_t last_heartbeat_ms;
-        
+
         // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
         uint32_t short_timer_ms;
-        
+
         uint32_t last_valid_rc_ms;
 
         //keeps track of the last valid rc as it relates to the AFS system
@@ -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;
@@ -428,7 +429,7 @@ private:
         // this is a 0..36000 value, or -1 for disabled
         int32_t hold_course_cd = -1;
 
-        // locked_course and locked_course_cd are used in stabilize mode 
+        // locked_course and locked_course_cd are used in stabilize mode
         // when ground steering is active, and for steering in auto-takeoff
         bool locked_course;
         float locked_course_err;
@@ -446,7 +447,7 @@ private:
         // the highest airspeed we have reached since entering AUTO. Used
         // to control ground takeoff
         float highest_airspeed;
-        
+
         // turn angle for next leg of mission
         float next_turn_angle {90};
 
@@ -455,13 +456,13 @@ private:
 
         // time when we first pass min GPS speed on takeoff
         uint32_t takeoff_speed_time_ms;
-        
+
         // distance to next waypoint
         float wp_distance;
-        
+
         // proportion to next waypoint
         float wp_proportion;
-        
+
         // last time is_flying() returned true in milliseconds
         uint32_t last_flying_ms;
 
@@ -556,7 +557,7 @@ private:
         AP_Vehicle::FixedWing::FlightStage last_flight_stage;
     } gear;
 #endif
-    
+
     struct {
         // on hard landings, only check once after directly a landing so you
         // don't trigger a crash when picking up the aircraft
@@ -580,7 +581,7 @@ private:
 
     // this controls throttle suppression in auto modes
     bool throttle_suppressed;
-	
+
     // reduce throttle to eliminate battery over-current
     int8_t  throttle_watt_limit_max;
     int8_t  throttle_watt_limit_min; // for reverse thrust
@@ -654,7 +655,7 @@ private:
         // previous target bearing, used to update sum_cd
         int32_t old_target_bearing_cd;
 
-        // Total desired rotation in a loiter.  Used for Loiter Turns commands. 
+        // Total desired rotation in a loiter.  Used for Loiter Turns commands.
         int32_t total_cd;
 
         // total angle completed in the loiter so far
@@ -951,7 +952,7 @@ private:
     void three_hz_loop(void);
 #if AP_AIRSPEED_AUTOCAL_ENABLE
     void airspeed_ratio_update(void);
-#endif 
+#endif
     void compass_save(void);
     void update_logging1(void);
     void update_logging2(void);
@@ -1099,7 +1100,7 @@ private:
     bool ekf_over_threshold();
     void failsafe_ekf_event();
     void failsafe_ekf_off_event(void);
-    
+
     enum class CrowMode {
         NORMAL,
         PROGRESSIVE,

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

@@ -130,7 +130,7 @@ void Plane::navigate()
 void Plane::calc_airspeed_errors()
 {
     float airspeed_measured = 0;
-    
+
     // we use the airspeed estimate function not direct sensor as TECS
     // may be using synthetic airspeed
     ahrs.airspeed_estimate(airspeed_measured);
@@ -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,22 +182,31 @@ 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) &&
-							 ((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();
-        if (is_positive(land_airspeed)) {
-            target_airspeed_cm = land_airspeed * 100;
+    } 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();
+                     if (is_positive(land_airspeed)) {
+                         target_airspeed_cm = land_airspeed * 100;
+                     } else {
+                         // fallover to normal airspeed
+                         target_airspeed_cm = aparm.airspeed_cruise_cm;
+                     }
         } else {
-            // fallover to normal airspeed
-            target_airspeed_cm = aparm.airspeed_cruise_cm;
+            // 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
+        // Normal airspeed target for all other cases
         target_airspeed_cm = aparm.airspeed_cruise_cm;
     }
 
@@ -215,7 +224,7 @@ void Plane::calc_airspeed_errors()
 
     // when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
 #if OFFBOARD_GUIDED == ENABLED
-    if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) { 
+    if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
         airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
     }
 #endif
@@ -308,7 +317,7 @@ void Plane::update_loiter(uint16_t radius)
 }
 
 /*
-  handle speed and height control in FBWB or CRUISE mode. 
+  handle speed and height control in FBWB or CRUISE mode.
   In this mode the elevator is used to change target altitude. The
   throttle is used to change target airspeed or throttle
  */
@@ -322,16 +331,16 @@ void Plane::update_fbwb_speed_height(void)
         dt = constrain_float(dt, 0.1, 0.15);
 
         target_altitude.last_elev_check_us = now;
-        
+
         float elevator_input = channel_pitch->get_control_in() / 4500.0f;
-    
+
         if (g.flybywire_elev_reverse) {
             elevator_input = -elevator_input;
         }
 
         int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100;
         change_target_altitude(alt_change_cm);
-        
+
         if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) {
             // the user has just released the elevator, lock in
             // the current altitude
@@ -350,14 +359,14 @@ void Plane::update_fbwb_speed_height(void)
             }
         }
 #endif
-        
+
         target_altitude.last_elevator_input = elevator_input;
     }
-    
+
     check_fbwb_minimum_altitude();
 
     altitude_error_cm = calc_altitude_error_cm();
-    
+
     calc_throttle();
     calc_nav_pitch();
 }
@@ -378,7 +387,7 @@ void Plane::setup_turn_angle(void)
         // work out the angle we need to turn through
         auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
     }
-}    
+}
 
 /*
   see if we have reached our loiter target
@@ -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()