From 05272f130d1140f6bd96a7883be486232fcb3e8b Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 21 May 2024 19:34:43 +0200 Subject: [PATCH 1/6] autotest: New tests Autotests for takeoffs have been added for Plane, covering AUTO and TAKEOFF mode takeoffs. An auxiliary `set_servo` method has been added to `vehicle_test_suite.py`. --- .../ArduPlane_Tests/TakeoffAuto1/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto2/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto3/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto4/catapult.txt | 13 + Tools/autotest/arduplane.py | 350 ++++++++++++++++++ Tools/autotest/vehicle_test_suite.py | 4 + 6 files changed, 406 insertions(+) create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt new file mode 100644 index 0000000000000..0aa9cd624db82 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.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 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.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 1.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_Tests/TakeoffAuto2/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt new file mode 100644 index 0000000000000..0aa9cd624db82 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.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 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.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 1.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_Tests/TakeoffAuto3/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt new file mode 100644 index 0000000000000..0aa9cd624db82 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.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 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.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 1.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_Tests/TakeoffAuto4/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt new file mode 100644 index 0000000000000..0aa9cd624db82 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.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 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.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 1.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 e57294be1121f..6356129df06b9 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4157,6 +4157,349 @@ def LandingDrift(self): self.wait_disarmed(timeout=180) + def TakeoffAuto1(self): + '''Test the behaviour of an AUTO takeoff, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_TYPE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_THR_MAX": 80.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto2(self): + '''Test the behaviour of an AUTO takeoff, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=0 + - TKOFF_THR_MAX > THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto3(self): + '''Test the behaviour of an AUTO takeoff, pt3. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")-1)) + + # Ensure that after that the aircraft does not go full throttle anymore. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")-10, operator.lt) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto4(self): + '''Test the behaviour of an AUTO takeoff, pt4. + + Conditions: + - ARSPD_USE=0 + - TKOFF_MODE=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Ensure that after that the aircraft still goes to maximum throttle. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffTakeoff1(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_MODE": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # 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("TKOFF_THR_MAX")) + + # 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("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, 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 TakeoffTakeoff2(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=1 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 80.0, + "TKOFF_ALT": 150.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # 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("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Check whether we've receded from 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("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MIN"))-1, 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 TakeoffTakeoff3(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt3. + + This is the same as case #1, but with disabled airspeed sensor. + + Conditions: + - ARSPD_USE=0 + - TKOFF_MODE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_MODE": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # 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("TKOFF_THR_MAX")) + + # 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("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, 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() @@ -5490,6 +5833,13 @@ def tests(self): self.AHRS_ORIENTATION, self.AHRSTrim, self.LandingDrift, + self.TakeoffAuto1, + self.TakeoffAuto2, + self.TakeoffAuto3, + self.TakeoffAuto4, + self.TakeoffTakeoff1, + self.TakeoffTakeoff2, + self.TakeoffTakeoff3, self.ForcedDCM, self.DCMFallback, self.MAVFTP, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 4677be0ab8afc..03265aa18dace 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5569,6 +5569,10 @@ def set_rc(self, chan, pwm, timeout=20): """Setup a simulated RC control to a PWM value""" self.set_rc_from_map({chan: pwm}, timeout=timeout) + def set_servo(self, chan, pwm): + """Replicate the functionality of MAVProxy: servo set """ + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=chan, p2=pwm) + def location_offset_ne(self, location, north, east): '''move location in metres''' print("old: %f %f" % (location.lat, location.lng)) From a15417a76487c287e8d51f0424ac9a3aa0c0f5c5 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 24 May 2024 11:38:42 +0200 Subject: [PATCH 2/6] AP_TECS: Fixes to reset state These concern the TAKEOFF flight stage and address #27147. * Logging metadata fixes * Disabled continuous TECS reset during takeoff * Fixed bug in reached_speed_takeoff calculation * Limited SPDWEIGHT=2 to only first part of takeoff climb * _post_TO_hgt_offset set to constant * Takeoff I-gain now defaults to 0 and is used * Use at least TRIM_THROTTLE during the climb * Updated description of TECS_TKOFF_IGAIN with new behaviour * Forced max throttle while below TKOFF_LVL_ALT * Added throttle constraints in no-airspeed mode Additionally, set_min_throttle() has been created, that allows one to set the minimum TECS throttle for the next update_pitch_throttle() loop. Additionally, the throttle limits system has been reworked. TECS will receive external throttle limits from Plane and will always take them into account and respect them. --- libraries/AP_TECS/AP_TECS.cpp | 148 +++++++++++++++++++--------- libraries/AP_TECS/AP_TECS.h | 20 ++++ libraries/AP_Vehicle/AP_FixedWing.h | 2 + 3 files changed, 126 insertions(+), 44 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 9307502caaf0b..8393a8b1e1c64 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -222,7 +222,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: TKOFF_IGAIN // @DisplayName: Controller integrator during takeoff - // @Description: This is the integrator gain on the control loop during takeoff. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values higher than TECS_INTEG_GAIN work best + // @Description: This is the integrator gain on the control loop during takeoff. Increase to increase the rate at which speed and height offsets are trimmed out. // @Range: 0.0 0.5 // @Increment: 0.02 // @User: Advanced @@ -775,7 +775,6 @@ void AP_TECS::_update_throttle_with_airspeed(void) // ensure we run at full throttle until we reach the target airspeed _throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state); } - _integTHR_state = integ_max; } else { _integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max); } @@ -823,7 +822,12 @@ void AP_TECS::_update_throttle_with_airspeed(void) #endif } - // Constrain throttle demand and record clipping + constrain_throttle(); + +} + +// Constrain throttle demand and record clipping +void AP_TECS::constrain_throttle() { if (_throttle_dem > _THRmaxf) { _thr_clip_status = clipStatus::MAX; _throttle_dem = _THRmaxf; @@ -839,9 +843,7 @@ float AP_TECS::_get_i_gain(void) { float i_gain = _integGain; if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { - if (!is_zero(_integGain_takeoff)) { - i_gain = _integGain_takeoff; - } + i_gain = _integGain_takeoff; } else if (_flags.is_doing_auto_land) { if (!is_zero(_integGain_land)) { i_gain = _integGain_land; @@ -889,18 +891,6 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi _throttle_dem = nomThr; } - if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { - const uint32_t now = AP_HAL::millis(); - if (_takeoff_start_ms == 0) { - _takeoff_start_ms = now; - } - const uint32_t dt = now - _takeoff_start_ms; - if (dt*0.001 < aparm.takeoff_throttle_max_t) { - _throttle_dem = _THRmaxf; - } - } else { - _takeoff_start_ms = 0; - } if (_flags.is_gliding) { _throttle_dem = 0.0f; return; @@ -914,6 +904,8 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); + + constrain_throttle(); } void AP_TECS::_detect_bad_descent(void) @@ -1084,17 +1076,17 @@ void AP_TECS::_update_pitch(void) // @Field: PEW: Potential energy weighting // @Field: KEW: Kinetic energy weighting // @Field: EBD: Energy balance demand - // @Field: EBE: Energy balance error + // @Field: EBE: Energy balance estimate // @Field: EBDD: Energy balance rate demand - // @Field: EBDE: Energy balance rate error + // @Field: EBDE: Energy balance rate estimate // @Field: EBDDT: Energy balance rate demand + Energy balance rate error*pitch_damping // @Field: Imin: Minimum integrator value // @Field: Imax: Maximum integrator value // @Field: I: Energy balance error integral // @Field: KI: Pitch demand kinetic energy integral - // @Field: pmin: Pitch min - // @Field: pmax: Pitch max - AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,pmin,pmax", + // @Field: tmin: Throttle min + // @Field: tmax: Throttle max + AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,tmin,tmax", "Qfffffffffffff", AP_HAL::micros64(), (double)SPE_weighting, @@ -1108,8 +1100,8 @@ void AP_TECS::_update_pitch(void) (double)integSEBdot_max, (double)_integSEBdot, (double)_integKE, - (double)_PITCHminf, - (double)_PITCHmaxf); + (double)_THRminf, + (double)_THRmaxf); } #endif } @@ -1168,22 +1160,25 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _hgt_dem = hgt_afe; _hgt_dem_in_prev = hgt_afe; _hgt_dem_in_raw = hgt_afe; - _TAS_dem_adj = _TAS_dem; - _flags.reset = true; - _post_TO_hgt_offset = _climb_rate * _hgt_dem_tconst; _flags.underspeed = false; _flags.badDescent = false; - + _post_TO_hgt_offset = _climb_rate_limit * _hgt_dem_tconst; // Replacement prevents oscillating hgt_rate_dem. + _TAS_dem_adj = _TAS_dem; _max_climb_scaler = 1.0f; _max_sink_scaler = 1.0f; - _pitch_demand_lpf.reset(_ahrs.get_pitch()); _pitch_measured_lpf.reset(_ahrs.get_pitch()); + + if (!_flag_have_reset_after_takeoff) { + _flags.reset = true; + _flag_have_reset_after_takeoff = true; + } } if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { // reset takeoff speed flag when not in takeoff _flags.reached_speed_takeoff = false; + _flag_have_reset_after_takeoff = false; } } @@ -1248,16 +1243,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _hgt_dem_in = _hgt_dem_in_raw; } - if (aparm.takeoff_throttle_max != 0 && - (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { - _THRmaxf = aparm.takeoff_throttle_max * 0.01f; - } else { - _THRmaxf = aparm.throttle_max * 0.01f; - } - _THRminf = aparm.throttle_min * 0.01f; - - // min of 1% throttle range to prevent a numerical error - _THRmaxf = MAX(_THRmaxf, _THRminf+0.01); + // Update the throttle limits. + _update_throttle_limits(); // work out the maximum and minimum pitch // if TECS_PITCH_{MAX,MIN} isn't set then use @@ -1308,9 +1295,6 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // note that this allows a flare pitch outside the normal TECS auto limits _PITCHmaxf = _land_pitch_max; } - - // and allow zero throttle - _THRminf = 0; } else if (_landing.is_on_approach()) { _PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min); _pitch_min_at_flare_entry = _PITCHminf; @@ -1335,7 +1319,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { - if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) { + if (!_flags.reached_speed_takeoff && _TAS_state >= _TASmin && _TASmin > 0) { // we have reached our target speed in takeoff, allow for // normal throttle control _flags.reached_speed_takeoff = true; @@ -1436,3 +1420,79 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } #endif } + +// set minimum throttle override, [-1, -1] range +// it is applicable for one control cycle only +void AP_TECS::set_throttle_min(const float thr_min) { + // Don't change the limit if it is already covered. + if (thr_min > _THRminf_ext) { + _THRminf_ext = thr_min; + } +} + +// set minimum throttle override, [0, -1] range +// it is applicable for one control cycle only +void AP_TECS::set_throttle_max(const float thr_max) { + // Don't change the limit if it is already covered. + if (thr_max < _THRmaxf_ext) { + _THRmaxf_ext = thr_max; + } +} + +void AP_TECS::_update_throttle_limits() { + + // Configure max throttle. + + // Read the maximum throttle limit. + _THRmaxf = aparm.throttle_max * 0.01f; + // If more max throttle is allowed during takeoff, use it. + if (aparm.takeoff_throttle_max*0.01f > _THRmaxf + && (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) + ) { + _THRmaxf = aparm.takeoff_throttle_max * 0.01f; + } + // In any case, constrain to the external safety limits. + _THRmaxf = MIN(_THRmaxf, _THRmaxf_ext); + + // Configure min throttle. + + // If less min throttle is allowed during takeoff, use it. + bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING; + use_takeoff_throttle = use_takeoff_throttle && (aparm.takeoff_mode == 1) && (aparm.takeoff_throttle_min != 0); + if ( use_takeoff_throttle ) { + _THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f); + } + else { // Otherwise, during normal situations let regular limit. + _THRminf = aparm.throttle_min * 0.01f; + } + // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff. + if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { + const uint32_t now = AP_HAL::millis(); + if (_takeoff_start_ms == 0) { + _takeoff_start_ms = now; + } + const uint32_t dt = now - _takeoff_start_ms; + if (dt*0.001 < aparm.takeoff_throttle_max_t) { + _THRminf = _THRmaxf; + } + } else { + _takeoff_start_ms = 0; + } + // If we are flaring, allow the throttle to go to 0. + if (_landing.is_flaring()) { + _THRminf = 0; + } + // In any case, constrain to the external safety limits. + _THRminf = MAX(_THRminf, _THRminf_ext); + + // Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors. + if (_THRmaxf < 1) { + _THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f); + } else { + _THRminf = MIN(_THRminf, _THRmaxf - 0.01f); + } + + // Reset the external throttle limits. + _THRminf_ext = -1.0f; + _THRmaxf_ext = 1.0f; +} \ No newline at end of file diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index fcd3fe97f9bfe..8ffff211fbf03 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -134,6 +134,14 @@ class AP_TECS { _pitch_max_limit = pitch_limit; } + // set minimum throttle override, [-1, -1] range + // it is applicable for one control cycle only + void set_throttle_min(const float thr_min); + + // set minimum throttle override, [0, -1] range + // it is applicable for one control cycle only + void set_throttle_max(const float thr_max); + // force use of synthetic airspeed for one loop void use_synthetic_airspeed(void) { _use_synthetic_airspeed_once = true; @@ -360,6 +368,9 @@ class AP_TECS { // Maximum and minimum floating point throttle limits float _THRmaxf; float _THRminf; + // Maximum and minimum throttle safety limits, set externally, typically by servos.cpp:apply_throttle_limits() + float _THRmaxf_ext = 1.0f; + float _THRminf_ext = -1.0f; // Maximum and minimum floating point pitch limits float _PITCHmaxf; @@ -419,6 +430,9 @@ class AP_TECS { // need to reset on next loop bool _need_reset; + // Checks if we reset at the beginning of takeoff. + bool _flag_have_reset_after_takeoff; + float _SKE_weighting; AP_Int8 _use_synthetic_airspeed; @@ -458,6 +472,9 @@ class AP_TECS { // Update Demanded Throttle Non-Airspeed void _update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg); + // Constrain throttle demand and record clipping + void constrain_throttle(); + // get integral gain which is flight_stage dependent float _get_i_gain(void); @@ -478,4 +495,7 @@ class AP_TECS { // current time constant float timeConstant(void) const; + + // Update the allowable throttle range. + void _update_throttle_limits(); }; diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 7aee38d5c5b35..29a0daf1f7a95 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -11,6 +11,8 @@ struct AP_FixedWing { AP_Int8 throttle_slewrate; AP_Int8 throttle_cruise; AP_Int8 takeoff_throttle_max; + AP_Int8 takeoff_throttle_min; + AP_Int8 takeoff_mode; AP_Int16 airspeed_min; AP_Int16 airspeed_max; AP_Float airspeed_cruise; From c1d113f215a662c3bdc89ad0ef2d927bd017f7ea Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 24 May 2024 11:38:42 +0200 Subject: [PATCH 3/6] ArduPlane: Added minimum throttle during TAKEOFF mode This is a rework so that servos.cpp is responsible for setting the throttle limits under more circumstances and always notifies TECS when it does so. Additionally, the TAKEOFF mode has been improved with a new parameters TKOFF_MODE and TKOFF_THR_MIN that extend the throttle behaviour. --- ArduPlane/Attitude.cpp | 5 +++++ ArduPlane/Parameters.cpp | 18 ++++++++++++++++- ArduPlane/Parameters.h | 2 ++ ArduPlane/Plane.h | 1 + ArduPlane/altitude.cpp | 4 ++-- ArduPlane/commands_logic.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 10 ++++----- ArduPlane/servos.cpp | 39 +++++++++++++++++++++++++++++------- ArduPlane/takeoff.cpp | 23 ++++++++++++++++++++- 9 files changed, 87 insertions(+), 17 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0996a50b725ab..788fe97dbada6 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -448,6 +448,10 @@ void Plane::stabilize() } +/* + * Set the throttle output. + * This is called by TECS-enabled flight modes, e.g. AUTO, GUIDED, etc. +*/ void Plane::calc_throttle() { if (aparm.throttle_cruise <= 1) { @@ -458,6 +462,7 @@ void Plane::calc_throttle() return; } + // Read the TECS throttle output and set it to the throttle channel. float commanded_throttle = TECS_controller.get_throttle_demand(); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 941dcba774ac0..3366ca2dd11d7 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -142,12 +142,28 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_THR_MAX_T // @DisplayName: Takeoff throttle maximum time - // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff without an airspeed sensor. If an airspeed sensor is being used then the throttle is set to maximum until the takeoff airspeed is reached. + // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff. // @Units: s // @Range: 0 10 // @Increment: 0.5 // @User: Standard ASCALAR(takeoff_throttle_max_t, "TKOFF_THR_MAX_T", 4), + + // @Param: TKOFF_THR_MIN + // @DisplayName: Takeoff minimum throttle + // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_MODE=1. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. + // @Units: % + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 60), + + // @Param: TKOFF_MODE + // @DisplayName: Takeoff mode + // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 0: During the takeoff, the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX). 1: During the takeoff TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. + // @Values: 0:Traditional,1:Throttle range + // @User: Advanced + ASCALAR(takeoff_mode, "TKOFF_MODE", 0), // @Param: TKOFF_TDRAG_ELEV // @DisplayName: Takeoff tail dragger elevator diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ba89c45772811..7eeb5459ec88d 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -356,6 +356,8 @@ class Parameters { k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, k_param_autotune_options, + k_param_takeoff_throttle_min, + k_param_takeoff_mode, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b1af208397ff3..4a78dd32e9e01 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1114,6 +1114,7 @@ class Plane : public AP_Vehicle { bool auto_takeoff_check(void); void takeoff_calc_roll(void); void takeoff_calc_pitch(void); + void takeoff_calc_throttle(const bool use_max_throttle=false); int8_t takeoff_tail_hold(void); int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 8b7970e6315d1..2844f2fc9164b 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -513,9 +513,9 @@ int32_t Plane::adjusted_altitude_cm(void) } /* - return home-relative altitude adjusted for ALT_OFFSET This is useful + return home-relative altitude adjusted for ALT_OFFSET. This is useful during long flights to account for barometer changes from the GCS, - or to adjust the flying height of a long mission + or to adjust the flying height of a long mission. */ int32_t Plane::adjusted_relative_altitude_cm(void) { diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4766cea5c1fba..58c7878a22ba2 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -391,7 +391,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) next_WP_loc.lat = home.lat + 10; next_WP_loc.lng = home.lng + 10; auto_state.takeoff_speed_time_ms = 0; - auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction + auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction. auto_state.height_below_takeoff_to_level_off_cm = 0; // Flag also used to override "on the ground" throttle disable diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 37d0203fcf413..23ae111439e63 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -22,11 +22,11 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Increment: 1 // @Units: m // @User: Standard - AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 5), + AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 10), // @Param: LVL_PITCH // @DisplayName: Takeoff mode altitude initial pitch - // @Description: This is the target pitch for the initial climb to TKOFF_LVL_ALT + // @Description: This is the target pitch during the takeoff. // @Range: 0 30 // @Increment: 1 // @Units: deg @@ -148,11 +148,11 @@ void ModeTakeoff::update() if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { //below TAKOFF_LVL_ALT - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); + plane.takeoff_calc_throttle(true); } else { - if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering + if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering #if AP_FENCE_ENABLED plane.fence.auto_enable_fence_after_takeoff(); #endif @@ -160,7 +160,7 @@ void ModeTakeoff::update() plane.calc_nav_pitch(); plane.calc_throttle(); } else { // still climbing to TAKEOFF_ALT; may be loitering - plane.calc_throttle(); + plane.takeoff_calc_throttle(); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index cc4d4dde104e3..6ac1baba77c53 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -499,47 +499,72 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) #endif // #if AP_BATTERY_WATT_MAX_ENABLED /* - Apply min/max limits to throttle + Apply min/max safety limits to throttle. */ float Plane::apply_throttle_limits(float throttle_in) { - // convert 0 to 100% (or -100 to +100) into PWM + // Pull the base throttle limits. + // These are usually set to map the ESC operating range. int8_t min_throttle = aparm.throttle_min.get(); int8_t max_throttle = aparm.throttle_max.get(); #if AP_ICENGINE_ENABLED - // apply idle governor + // Apply idle governor. g2.ice_control.update_idle_governor(min_throttle); #endif + // If reverse thrust is enabled not allowed right now, the minimum throttle must not fall below 0. if (min_throttle < 0 && !allow_reverse_thrust()) { // reverse thrust is available but inhibited. min_throttle = 0; } - const bool use_takeoff_throttle_max = + // 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); - if (use_takeoff_throttle_max) { + if (use_takeoff_throttle) { if (aparm.takeoff_throttle_max != 0) { + // Replace max throttle with the takeoff max throttle setting. + // This is typically done to protect against long intervals of large power draw. + // 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. + if (aparm.takeoff_mode.get() == 0 || !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()); + } else if (aparm.takeoff_mode.get() == 1) { // 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()); + } + } } else if (landing.is_flaring()) { + // Allow throttle cutoff when flaring. + // This is to allow the aircraft to bleed speed faster and land with a shut off thruster. min_throttle = 0; } - // compensate for battery voltage drop + // 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); #if AP_BATTERY_WATT_MAX_ENABLED - // apply watt limiter + // Ensure that the power draw limits are not exceeded. throttle_watt_limiter(min_throttle, max_throttle); #endif + // Do a sanity check on them. Constrain down if necessary. + min_throttle = MIN(min_throttle, max_throttle); + + // Let TECS know about the updated throttle limits. + TECS_controller.set_throttle_min(0.01f*min_throttle); + TECS_controller.set_throttle_max(0.01f*max_throttle); return constrain_float(throttle_in, min_throttle, max_throttle); } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index faa220412fd91..d6e8ddc4f533d 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -219,7 +219,28 @@ void Plane::takeoff_calc_pitch(void) } /* - * get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off + * Set the throttle limits to run at during a takeoff. + */ +void Plane::takeoff_calc_throttle(const bool use_max_throttle) { + // This setting will take effect at the next run of TECS::update_pitch_throttle(). + + // Set the maximum throttle limit. + if (aparm.takeoff_throttle_max != 0) { + TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max); + } + + // Set the minimum throttle limit. + if (aparm.takeoff_mode==0 || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. + TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max); + } 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); + } + } + calc_throttle(); +} + +/* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off */ int16_t Plane::get_takeoff_pitch_min_cd(void) { From 7c95fac422dafda8708957d0c2f4d2c88b859c68 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 25 Jul 2024 21:28:59 +0200 Subject: [PATCH 4/6] Plane: Converted paramter TKOFF_MODE into TKOFF_OPTIONS --- ArduPlane/Parameters.cpp | 12 ++++++------ ArduPlane/Parameters.h | 2 +- ArduPlane/servos.cpp | 5 +++-- ArduPlane/takeoff.cpp | 3 ++- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 3366ca2dd11d7..8092c317a20f2 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -151,19 +151,19 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_THR_MIN // @DisplayName: Takeoff minimum throttle - // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_MODE=1. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. + // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_OPTIONS bit 0 is set. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. // @Units: % // @Range: 0 100 // @Increment: 1 // @User: Standard ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 60), - // @Param: TKOFF_MODE - // @DisplayName: Takeoff mode - // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 0: During the takeoff, the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX). 1: During the takeoff TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. - // @Values: 0:Traditional,1:Throttle range + // @Param: TKOFF_OPTIONS + // @DisplayName: Takeoff options + // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. + // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. // @User: Advanced - ASCALAR(takeoff_mode, "TKOFF_MODE", 0), + ASCALAR(takeoff_options, "TKOFF_OPTIONS", 0), // @Param: TKOFF_TDRAG_ELEV // @DisplayName: Takeoff tail dragger elevator diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7eeb5459ec88d..b960e5ffb7d8a 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -357,7 +357,7 @@ class Parameters { k_param_takeoff_throttle_max_t, k_param_autotune_options, k_param_takeoff_throttle_min, - k_param_takeoff_mode, + k_param_takeoff_options, }; AP_Int16 format_version; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 6ac1baba77c53..ab3dd86ae2508 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -536,10 +536,11 @@ float Plane::apply_throttle_limits(float throttle_in) } // Do not allow min throttle to go below a lower threshold. // This is typically done to protect against premature stalls close to the ground. - if (aparm.takeoff_mode.get() == 0 || !ahrs.using_airspeed_sensor()) { + 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()); - } else if (aparm.takeoff_mode.get() == 1) { // Use a throttle range through the takeoff. + } 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 d6e8ddc4f533d..d45651fbfe885 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -230,7 +230,8 @@ void Plane::takeoff_calc_throttle(const bool use_max_throttle) { } // Set the minimum throttle limit. - if (aparm.takeoff_mode==0 || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff 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); } else { // TKOFF_MODE == 1, allow for a throttle range. if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN. From fa030015e8fcf055b0ce46ac2f85abcc7461b837 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 25 Jul 2024 21:29:22 +0200 Subject: [PATCH 5/6] AP_TECS: Converted parameter TKOFF_MODE to TKOFF_OPTIONS --- libraries/AP_TECS/AP_TECS.cpp | 3 ++- libraries/AP_Vehicle/AP_FixedWing.h | 7 ++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 8393a8b1e1c64..db42c9624f91c 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -1458,7 +1458,8 @@ void AP_TECS::_update_throttle_limits() { // If less min throttle is allowed during takeoff, use it. bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING; - use_takeoff_throttle = use_takeoff_throttle && (aparm.takeoff_mode == 1) && (aparm.takeoff_throttle_min != 0); + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + use_takeoff_throttle = use_takeoff_throttle && (use_throttle_range == 1) && (aparm.takeoff_throttle_min != 0); if ( use_takeoff_throttle ) { _THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f); } diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 29a0daf1f7a95..7f6aa03eb9010 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -12,7 +12,7 @@ struct AP_FixedWing { AP_Int8 throttle_cruise; AP_Int8 takeoff_throttle_max; AP_Int8 takeoff_throttle_min; - AP_Int8 takeoff_mode; + AP_Int32 takeoff_options; AP_Int16 airspeed_min; AP_Int16 airspeed_max; AP_Float airspeed_cruise; @@ -50,4 +50,9 @@ struct AP_FixedWing { LAND = 4, ABORT_LANDING = 7 }; + + // Bitfields of TKOFF_OPTIONS + enum class TakeoffOption { + THROTTLE_RANGE = (1U << 0), // Unset: Max throttle. Set: Throttle range. + }; }; From 04ec2b983067258598a04c7ffbd8d39ca2748af5 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 25 Jul 2024 21:29:35 +0200 Subject: [PATCH 6/6] autotest: Converted parameter TKOFF_MODE to TKOFF_OPTIONS --- Tools/autotest/arduplane.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 6356129df06b9..4958f1504672d 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4162,7 +4162,7 @@ def TakeoffAuto1(self): Conditions: - ARSPD_USE=1 - - TKOFF_TYPE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX < THR_MAX ''' @@ -4207,7 +4207,7 @@ def TakeoffAuto2(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX > THR_MAX ''' @@ -4252,7 +4252,7 @@ def TakeoffAuto3(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=1 + - TKOFF_OPTIONS[0]=1 ''' self.customise_SITL_commandline( @@ -4264,7 +4264,7 @@ def TakeoffAuto3(self): "ARSPD_USE": 1.0, "THR_MAX": 80.0, "THR_MIN": 0.0, - "TKOFF_MODE": 1.0, + "TKOFF_OPTIONS": 1.0, "TKOFF_THR_MAX": 100.0, "TKOFF_THR_MINACC": 3.0, "TECS_PITCH_MAX": 35.0, @@ -4301,7 +4301,7 @@ def TakeoffAuto4(self): Conditions: - ARSPD_USE=0 - - TKOFF_MODE=1 + - TKOFF_OPTIONS[0]=1 ''' self.customise_SITL_commandline( @@ -4313,7 +4313,7 @@ def TakeoffAuto4(self): "ARSPD_USE": 0.0, "THR_MAX": 80.0, "THR_MIN": 0.0, - "TKOFF_MODE": 1.0, + "TKOFF_OPTIONS": 1.0, "TKOFF_THR_MAX": 100.0, "TKOFF_THR_MINACC": 3.0, "TECS_PITCH_MAX": 35.0, @@ -4352,7 +4352,7 @@ def TakeoffTakeoff1(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX < THR_MAX ''' @@ -4366,7 +4366,7 @@ def TakeoffTakeoff1(self): "THR_MAX": 100.0, "TKOFF_LVL_ALT": 30.0, "TKOFF_ALT": 100.0, - "TKOFF_MODE": 0.0, + "TKOFF_OPTIONS": 0.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, "TECS_PITCH_MAX": 35.0, @@ -4402,7 +4402,7 @@ def TakeoffTakeoff2(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=1 + - TKOFF_OPTIONS[0]=1 - TKOFF_THR_MAX < THR_MAX ''' @@ -4416,7 +4416,7 @@ def TakeoffTakeoff2(self): "THR_MAX": 100.0, "TKOFF_LVL_ALT": 80.0, "TKOFF_ALT": 150.0, - "TKOFF_MODE": 1.0, + "TKOFF_OPTIONS": 1.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, "TECS_PITCH_MAX": 35.0, @@ -4455,7 +4455,7 @@ def TakeoffTakeoff3(self): Conditions: - ARSPD_USE=0 - - TKOFF_MODE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX < THR_MAX ''' @@ -4469,7 +4469,7 @@ def TakeoffTakeoff3(self): "THR_MAX": 100.0, "TKOFF_LVL_ALT": 30.0, "TKOFF_ALT": 100.0, - "TKOFF_MODE": 0.0, + "TKOFF_OPTIONS": 0.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, "TECS_PITCH_MAX": 35.0,