Plane: fixed bug in pullup and loiter to alt code

ArduPlane/pullup.cpp

@@ -115,7 +115,7 @@ bool GliderPullup::verify_pullup(void)
     switch (stage) {
     case Stage::WAIT_AIRSPEED: {
         float aspeed;
-        if (ahrs.airspeed_estimate(aspeed) && aspeed > airspeed_start) {
+        if (ahrs.airspeed_estimate(aspeed) && (aspeed > airspeed_start || ahrs.pitch_sensor*0.01 > pitch_start)) {
             gcs().send_text(MAV_SEVERITY_INFO, "Pullup airspeed %.1fm/s alt %.1fm AMSL", aspeed, current_loc.alt*0.01);
             stage = Stage::WAIT_PITCH;
         }

Tools/autotest/ArduPlane_Tests/GliderPullup/glider-pullup-mission.txt

@@ -1,3 +1,6 @@
 QGC WPL 110
 0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.000000	1
-1	0	0	83	2000.000000	10.000000	30.000000	0.000000	0.000000	0.000000	0.000000	1
+1	0	0	83	5700.000000	10.000000	30.000000	0.000000	0.000000	0.000000	0.000000	1
+2	0	3	31	0.000000	0.000000	0.000000	0.000000	-35.413030	149.172036	1500.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.391115	149.169040	250.000000	1
+4	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.362803	149.165169	0.000000	1

Tools/autotest/arduplane.py

@@ -5533,6 +5533,7 @@ def RunMissionScript(self):
 
     def MAV_CMD_GUIDED_CHANGE_ALTITUDE(self):
         '''test handling of MAV_CMD_GUIDED_CHANGE_ALTITUDE'''
+        self.install_terrain_handlers_context()
         self.start_subtest("set home relative altitude")
         self.takeoff(30, relative=True)
         self.change_mode('GUIDED')
@@ -6198,14 +6199,14 @@ def GliderPullup(self):
         self.progress("Start balloon lift")
         self.set_servo(6, 2000)
 
-        self.wait_text("Reached altitude", check_context=True, timeout=300)
+        self.wait_text("Reached altitude", check_context=True, timeout=1000)
         self.wait_text("Start pullup airspeed", check_context=True)
         self.wait_text("Pullup airspeed", check_context=True)
         self.wait_text("Pullup pitch", check_context=True)
         self.wait_text("Pullup level", check_context=True)
-        self.wait_text("Mission complete, changing mode to RTL", check_context=True)
-
-        self.fly_home_land_and_disarm(timeout=400)
+        self.wait_text("Loiter to alt complete", check_context=True, timeout=1000)
+        self.wait_text("Flare", check_context=True, timeout=400)
+        self.wait_text("Auto disarmed", check_context=True, timeout=200)
 
     def BadRollChannelDefined(self):
         '''ensure we don't die with a  bad Roll channel defined'''

Tools/autotest/default_params/glider.parm

@@ -36,7 +36,7 @@ ARSPD_USE        1
 HOME_RESET_ALT -1
 
 TECS_INTEG_GAIN 0.25
-TECS_SPDWEIGHT 2
+TECS_SPDWEIGHT 1.7 # allow some altitude control
 TECS_PTCH_DAMP 0.6
 TECS_SINK_MAX 8.5
 TECS_PITCH_MAX 10

Tools/autotest/vehicle_test_suite.py

@@ -7305,13 +7305,13 @@ def validator(value2, target2=None):
 
         self.wait_and_maintain(
             value_name="Altitude",
-            target=altitude_min,
+            target=(altitude_min + altitude_max)*0.5,
             current_value_getter=lambda: self.get_altitude(
                 relative=relative,
                 timeout=timeout,
                 altitude_source=altitude_source,
             ),
-            accuracy=(altitude_max - altitude_min),
+            accuracy=(altitude_max - altitude_min)*0.5,
             validator=lambda value2, target2: validator(value2, target2),
             timeout=timeout,
             **kwargs
@@ -7436,8 +7436,8 @@ def wait_and_maintain(self, value_name, target, current_value_getter, validator=
             )
         return self.wait_and_maintain_range(
             value_name,
-            minimum=target - accuracy/2,
-            maximum=target + accuracy/2,
+            minimum=target - accuracy,
+            maximum=target + accuracy,
             current_value_getter=current_value_getter,
             validator=validator,
             timeout=timeout,

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -339,6 +339,7 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
 
     // Waypoint capture status is always false during waypoint following
     _WPcircle = false;
+    _last_loiter.reached_loiter_target_ms = 0;
 
     _bearing_error = Nu; // bearing error angle (radians), +ve to left of track
 
@@ -348,6 +349,8 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex
 // update L1 control for loitering
 void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_t loiter_direction)
 {
+    const float radius_unscaled = radius;
+
     Location _current_loc;
 
     // scale loiter radius with square of EAS2TAS to allow us to stay
@@ -442,18 +445,43 @@ void AP_L1_Control::update_loiter(const Location &center_WP, float radius, int8_
     // Perform switchover between 'capture' and 'circle' modes at the
     // point where the commands cross over to achieve a seamless transfer
     // Only fly 'capture' mode if outside the circle
+    const uint32_t now_ms = AP_HAL::millis();
     if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
         _latAccDem = latAccDemCap;
-        _WPcircle = false;
+
+        /*
+          if we were previously on the circle and the target has not
+          changed then keep _WPcircle true. This prevents
+          reached_loiter_target() from going false due to a gust of
+          wind or an unachievable loiter radius
+         */
+        if (_WPcircle &&
+            _last_loiter.reached_loiter_target_ms != 0 &&
+            now_ms - _last_loiter.reached_loiter_target_ms < 200U &&
+            loiter_direction == _last_loiter.direction &&
+            is_equal(radius_unscaled, _last_loiter.radius) &&
+            center_WP.same_loc_as(_last_loiter.center_WP)) {
+            // same location, within 200ms, keep the _WPcircle status as true
+            _last_loiter.reached_loiter_target_ms = now_ms;
+        } else {
+            _WPcircle = false;
+            _last_loiter.reached_loiter_target_ms = 0;
+        }
+
         _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
         _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
     } else {
         _latAccDem = latAccDemCirc;
         _WPcircle = true;
+        _last_loiter.reached_loiter_target_ms = now_ms;
         _bearing_error = 0.0f; // bearing error (radians), +ve to left of track
         _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
     }
 
+    _last_loiter.radius = radius_unscaled;
+    _last_loiter.direction = loiter_direction;
+    _last_loiter.center_WP = center_WP;
+
     _data_is_stale = false; // status are correctly updated with current waypoint data
 }
 
@@ -487,6 +515,7 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
 
     // Waypoint capture status is always false during heading hold
     _WPcircle = false;
+    _last_loiter.reached_loiter_target_ms = 0;
 
     _crosstrack_error = 0;
 
@@ -510,6 +539,7 @@ void AP_L1_Control::update_level_flight(void)
 
     // Waypoint capture status is always false during heading hold
     _WPcircle = false;
+    _last_loiter.reached_loiter_target_ms = 0;
 
     _latAccDem = 0;
 

libraries/AP_L1_Control/AP_L1_Control.h

@@ -124,6 +124,14 @@ class AP_L1_Control : public AP_Navigation {
 
     AP_Float _loiter_bank_limit;
 
+    // remember reached_loiter_target decision
+    struct {
+        uint32_t reached_loiter_target_ms;
+        float radius;
+        int8_t direction;
+        Location center_WP;
+    } _last_loiter;
+
     bool _reverse = false;
     float get_yaw() const;
     int32_t get_yaw_sensor() const;