From 39b78d55cc789c74e6345fbfd21d640ab1286e87 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Mon, 29 Jul 2024 21:31:43 +0200 Subject: [PATCH 1/3] Plane: Fixed unhandled TKOFF_THR_MAX=0 case --- ArduPlane/servos.cpp | 7 ++++++- ArduPlane/takeoff.cpp | 3 ++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index ab3dd86ae2508..45eba977a3dff 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -534,12 +534,17 @@ float Plane::apply_throttle_limits(float throttle_in) // Or (in contrast) to give some extra throttle during the initial climb. max_throttle = aparm.takeoff_throttle_max.get(); } + // Do not allow min throttle to go below a lower threshold. // This is typically done to protect against premature stalls close to the ground. const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); if (!use_throttle_range || !ahrs.using_airspeed_sensor()) { // Use a constant max throttle throughout the takeoff or when airspeed readings are not available. - min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get()); + if (aparm.takeoff_throttle_max.get() == 0) { + min_throttle = MAX(min_throttle, aparm.throttle_max.get()); + } else { + min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get()); + } } else if (use_throttle_range) { // Use a throttle range through the takeoff. if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1. min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get()); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index d45651fbfe885..e6c26076a5b88 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -232,7 +232,8 @@ void Plane::takeoff_calc_throttle(const bool use_max_throttle) { // Set the minimum throttle limit. const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); if (!use_throttle_range || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. - TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max); + float min_throttle = (aparm.takeoff_throttle_max != 0) ? 0.01f*aparm.takeoff_throttle_max : 0.01f*aparm.throttle_max; + TECS_controller.set_throttle_min(min_throttle); } else { // TKOFF_MODE == 1, allow for a throttle range. if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN. TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min); From 0f8b77411a754bd1180647bc4ad3c45e07a18cfb Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Mon, 29 Jul 2024 21:55:39 +0200 Subject: [PATCH 2/3] autotest: Added airspeedless takeoff test with stock parameters --- Tools/autotest/arduplane.py | 41 +++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2af157ab9c8ce..be72ceaa31527 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4714,6 +4714,46 @@ def TakeoffTakeoff3(self): self.fly_home_land_and_disarm() + def TakeoffTakeoff4(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt4. + + This is the same as case #3, but with almost stock parameters and without a catapult. + + Conditions: + - ARSPD_USE=0 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("THR_MAX"))-10, operator.ge) + + # Check whether we're still at max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("THR_MAX"))-10, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -6062,6 +6102,7 @@ def tests(self): self.TakeoffTakeoff1, self.TakeoffTakeoff2, self.TakeoffTakeoff3, + self.TakeoffTakeoff4, self.ForcedDCM, self.DCMFallback, self.MAVFTP, From df6c206b2dae9f44f8791bdfaff76734b4d3b1a9 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 30 Jul 2024 12:13:29 +0200 Subject: [PATCH 3/3] Plane: Revert min throttle during transitions --- ArduPlane/servos.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 45eba977a3dff..4f7b1c44506d6 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -521,12 +521,10 @@ float Plane::apply_throttle_limits(float throttle_in) // Query the conditions where TKOFF_THR_MAX applies. const bool use_takeoff_throttle = -#if HAL_QUADPLANE_ENABLED - quadplane.in_transition() || -#endif (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); + // Handle throttle limits for takeoff conditions. if (use_takeoff_throttle) { if (aparm.takeoff_throttle_max != 0) { // Replace max throttle with the takeoff max throttle setting. @@ -534,7 +532,6 @@ float Plane::apply_throttle_limits(float throttle_in) // Or (in contrast) to give some extra throttle during the initial climb. max_throttle = aparm.takeoff_throttle_max.get(); } - // Do not allow min throttle to go below a lower threshold. // This is typically done to protect against premature stalls close to the ground. const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); @@ -556,6 +553,15 @@ float Plane::apply_throttle_limits(float throttle_in) min_throttle = 0; } + // Handle throttle limits for transition conditions. +#if HAL_QUADPLANE_ENABLED + if (quadplane.in_transition()) { + if (aparm.takeoff_throttle_max != 0) { + max_throttle = aparm.takeoff_throttle_max.get(); + } + } +#endif + // Compensate the limits for battery voltage drop. // This relaxes the limits when the battery is getting depleted. g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle);