From 5d2e47283c64ebcca1c12df2b4122cfa1db2cd0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 18 Dec 2024 19:42:35 +1100 Subject: [PATCH 1/2] autotest: Created test for unsuccessful takeoff level off --- .../TakeoffBadLevelOff/flaps_tkoff_50.txt | 13 ++++ Tools/autotest/arduplane.py | 70 +++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt b/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt new file mode 100644 index 0000000000000..c2f52f8929053 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 50.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index a724a9a45a38d..560372ca83ec5 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4911,6 +4911,74 @@ def TakeoffGround(self): self.fly_home_land_and_disarm() + def TakeoffIdleThrottle(self): + '''Apply idle throttle before takeoff.''' + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "TKOFF_THR_IDLE": 20.0, + "TKOFF_THR_MINSPD": 3.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Ensure that the throttle rises to idle throttle. + expected_idle_throttle = 1000+10*self.get_parameter("TKOFF_THR_IDLE") + self.assert_servo_channel_range(3, expected_idle_throttle-10, expected_idle_throttle+10) + + # Launch the catapult + self.set_servo(6, 1000) + + self.delay_sim_time(5) + self.change_mode('RTL') + + self.fly_home_land_and_disarm() + + def TakeoffBadLevelOff(self): + '''Ensure that the takeoff can be completed under 0 pitch demand.''' + ''' + When using no airspeed, the pitch level-off will eventually command 0 + pitch demand. Ensure that the plane can climb the final 2m to deem the + takeoff complete. + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "PTCH_TRIM_DEG": -10.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + "TKOFF_ALT": 50.0, + "TKOFF_DIST": 1000.0, + "TKOFF_THR_MAX": 75.0, + "TKOFF_THR_MINACC": 3.0, + }) + + self.load_mission("flaps_tkoff_50.txt") + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we've reached the takeoff altitude. + target_alt = 50 + self.wait_altitude(target_alt-1, target_alt+1, relative=True, timeout=30) + + self.delay_sim_time(5) + + self.disarm_vehicle(force=True) + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -6503,6 +6571,8 @@ def tests1b(self): self.TakeoffTakeoff4, self.TakeoffTakeoff5, self.TakeoffGround, + self.TakeoffIdleThrottle, + self.TakeoffBadLevelOff, self.ForcedDCM, self.DCMFallback, self.MAVFTP, From 6448c7f1be7c75afbe3f1a11d08dce7f3458155e Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 22 Nov 2024 15:36:46 +0100 Subject: [PATCH 2/2] Plane: Added check for takeoff level-off timeout When an airspeed sensor is not used, during a takeoff, the pitch angle is asymptotically driven to 0 as the takeoff altitude is approached. Some airplanes will then stop climbing and fail to reach altitude. To prevent an indefinite wait for the takeoff altitude to be reached, a dedicated level-off timeout has been introduced. --- ArduPlane/Plane.h | 3 ++- ArduPlane/commands_logic.cpp | 5 ++++- ArduPlane/mode_takeoff.cpp | 7 +++++++ ArduPlane/takeoff.cpp | 19 +++++++++++++++++-- 4 files changed, 30 insertions(+), 4 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c7c5f6a17d9cb..808ff1574c71a 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -453,7 +453,7 @@ class Plane : public AP_Vehicle { float throttle_lim_max; float throttle_lim_min; uint32_t throttle_max_timer_ms; - // Good candidate for keeping the initial time for TKOFF_THR_MAX_T. + uint32_t level_off_start_time_ms; } takeoff_state; // ground steering controller state @@ -1147,6 +1147,7 @@ class Plane : public AP_Vehicle { int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); bool check_takeoff_timeout(void); + bool check_takeoff_timeout_level_off(void); // avoidance_adsb.cpp void avoidance_adsb_update(void); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 90c3e024800c6..4b9dd736610ac 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -587,7 +587,10 @@ bool Plane::verify_takeoff() // see if we have reached takeoff altitude int32_t relative_alt_cm = adjusted_relative_altitude_cm(); - if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { + if ( + relative_alt_cm > auto_state.takeoff_altitude_rel_cm || // altitude reached + plane.check_takeoff_timeout_level_off() // pitch level-off maneuver has timed out + ) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm", (double)(relative_alt_cm*0.01f)); steer_state.hold_course_cd = -1; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 4bca83cccd874..42cdcb3d45673 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -130,6 +130,7 @@ void ModeTakeoff::update() gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); plane.takeoff_state.start_time_ms = millis(); + plane.takeoff_state.level_off_start_time_ms = 0; plane.takeoff_state.throttle_max_timer_ms = millis(); takeoff_mode_setup = true; plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. @@ -168,6 +169,12 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } + // If we have timed-out on the attempt to close the final few meters + // during pitch level-off, continue to NORMAL flight stage. + if (plane.check_takeoff_timeout_level_off()) { + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { //below TKOFF_ALT plane.takeoff_calc_roll(); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index dda412d5ee05f..d7617e2f5696b 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void) takeoff_state.launchTimerStarted = false; takeoff_state.last_tkoff_arm_time = 0; takeoff_state.start_time_ms = now; + takeoff_state.level_off_start_time_ms = 0; takeoff_state.throttle_max_timer_ms = now; steer_state.locked_course_err = 0; // use current heading without any error offset return true; @@ -316,6 +317,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) // make a note of that altitude to use it as a start height for scaling gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100)); auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm; + takeoff_state.level_off_start_time_ms = AP_HAL::millis(); } } } @@ -376,9 +378,8 @@ void Plane::landing_gear_update(void) #endif /* - check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout + check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred */ - bool Plane::check_takeoff_timeout(void) { if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { @@ -400,3 +401,17 @@ bool Plane::check_takeoff_timeout(void) return false; } +/* + check if the pitch level-off time has expired; returns true if timeout has occurred +*/ +bool Plane::check_takeoff_timeout_level_off(void) +{ + if (takeoff_state.level_off_start_time_ms > 0) { + // A takeoff is in progress. + uint32_t now = AP_HAL::millis(); + if ((now - takeoff_state.level_off_start_time_ms) > (uint32_t)(1000U * g.takeoff_pitch_limit_reduction_sec)) { + return true; + } + } + return false; +}