-
Notifications
You must be signed in to change notification settings - Fork 17.6k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Plane: Takeoff improvements #27758
Plane: Takeoff improvements #27758
Conversation
9883163
to
33e81d9
Compare
ArduPlane/mode_takeoff.cpp
Outdated
@@ -153,7 +153,7 @@ void ModeTakeoff::update() | |||
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) || climb_complete) { //within 2m of TKOFF_ALT, or above and loitering |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't believe this can cause adverse effects.
Once the plane reached TKOFF_ALT
, we can be confident that we can let TECS take over, and stop calling takeoff_calc_pitch()
and takeoff_calc_throttle()
.
ArduPlane/takeoff.cpp
Outdated
if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF || | ||
control_mode == &mode_takeoff) { | ||
// during takeoff we want to prioritise roll control over | ||
// pitch. Apply a reduction in pitch demand if our roll is | ||
// significantly off. The aim of this change is to | ||
// increase the robustness of hand launches, particularly | ||
// in cross-winds. If we start to roll over then we reduce | ||
// pitch demand until the roll recovers | ||
float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90)); | ||
float reduction = sq(cosf(roll_error_rad)); | ||
nav_pitch_cd *= reduction; | ||
} | ||
// during takeoff we want to prioritise roll control over | ||
// pitch. Apply a reduction in pitch demand if our roll is | ||
// significantly off. The aim of this change is to | ||
// increase the robustness of hand launches, particularly | ||
// in cross-winds. If we start to roll over then we reduce | ||
// pitch demand until the roll recovers | ||
float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90)); | ||
float reduction = sq(cosf(roll_error_rad)); | ||
nav_pitch_cd *= reduction; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The only cases I could find where we are calling takeoff_calc_pitch()
are in mode TAKEOFF and in mode AUTO when we are serving a takeoff or aborted landing items.
Thus the if-check is redundant.
@@ -1156,7 +1158,6 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) | |||
_pitch_measured_lpf.reset(_ahrs.get_pitch()); | |||
|
|||
} else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { | |||
_PITCHminf = CentiDegreesToRadians(ptchMinCO_cd); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is moved to the dedicated pitch limit method.
libraries/AP_TECS/AP_TECS.cpp
Outdated
_flags.reset = false; | ||
|
||
// Initialise states and variables if DT > 0.2 second or in climbout | ||
_need_reset = _need_reset || (_flag_pitch_forced && _flag_throttle_forced); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If TECS isn't allowed to control its own pitch and throttle it is effectively inactive.
Have it reset to prevent unnecessary windups.
_PITCHmaxf = _pitch_max; | ||
} | ||
|
||
if (_pitch_min == 0) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Previously this test was _pitch_min >= 0
, but if I understand correctly it was the wrong check.
if (auto_state.highest_airspeed < g.takeoff_rotate_speed) { | ||
// we have not reached rotate speed, use the specified takeoff target pitch angle | ||
nav_pitch_cd = int32_t(100.0f * mode_takeoff.ground_pitch); | ||
return; | ||
// First see if TKOFF_ROTATE_SPD applies. | ||
// This will set the pitch for the first portion of the takeoff, up until cruise speed is reached. | ||
if (g.takeoff_rotate_speed > 0) { | ||
// A non-zero rotate speed is recommended for ground takeoffs. | ||
if (auto_state.highest_airspeed < g.takeoff_rotate_speed) { | ||
// We have not reached rotate speed, use the specified takeoff target pitch angle. | ||
nav_pitch_cd = int32_t(100.0f * mode_takeoff.ground_pitch); | ||
TECS_controller.set_pitch_min(0.01f*nav_pitch_cd); | ||
TECS_controller.set_pitch_max(0.01f*nav_pitch_cd); | ||
return; | ||
} else if (gps.ground_speed() <= (float)aparm.airspeed_cruise) { | ||
// If rotate speed applied, gradually transition from TKOFF_GND_PITCH to the climb angle. | ||
// This is recommended for ground takeoffs, so delay rotation until ground speed indicates adequate airspeed. | ||
const uint16_t min_pitch_cd = 500; // Set a minimum of 5 deg climb angle. | ||
nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd; | ||
nav_pitch_cd = constrain_int32(nav_pitch_cd, min_pitch_cd, auto_state.takeoff_pitch_cd); | ||
TECS_controller.set_pitch_min(0.01f*nav_pitch_cd); | ||
TECS_controller.set_pitch_max(0.01f*nav_pitch_cd); | ||
return; | ||
} | ||
} | ||
|
||
// We are now past the rotation. | ||
// Initialize pitch limits for TECS. | ||
int16_t pitch_min_cd = get_takeoff_pitch_min_cd(); | ||
bool pitch_clipped_max = false; | ||
|
||
// If we're using an airspeed sensor, we consult TECS. | ||
if (ahrs.using_airspeed_sensor()) { | ||
int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd(); | ||
calc_nav_pitch(); | ||
if (nav_pitch_cd < takeoff_pitch_min_cd) { | ||
nav_pitch_cd = takeoff_pitch_min_cd; | ||
// At any rate, we don't want to go lower than the minimum pitch bound. | ||
if (nav_pitch_cd < pitch_min_cd) { | ||
nav_pitch_cd = pitch_min_cd; | ||
} | ||
} else { | ||
if (g.takeoff_rotate_speed > 0) { | ||
// Rise off ground takeoff so delay rotation until ground speed indicates adequate airspeed | ||
nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd; | ||
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); | ||
} else { | ||
// Doing hand or catapult launch so need at least 5 deg pitch to prevent initial height loss | ||
nav_pitch_cd = MAX(auto_state.takeoff_pitch_cd, 500); | ||
} | ||
// If not, we will use the minimum allowed angle. | ||
nav_pitch_cd = pitch_min_cd; | ||
|
||
pitch_clipped_max = true; | ||
} | ||
|
||
// Check if we have trouble with roll control. | ||
if (aparm.stall_prevention != 0) { | ||
if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF || | ||
control_mode == &mode_takeoff) { | ||
// during takeoff we want to prioritise roll control over | ||
// pitch. Apply a reduction in pitch demand if our roll is | ||
// significantly off. The aim of this change is to | ||
// increase the robustness of hand launches, particularly | ||
// in cross-winds. If we start to roll over then we reduce | ||
// pitch demand until the roll recovers | ||
float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90)); | ||
float reduction = sq(cosf(roll_error_rad)); | ||
nav_pitch_cd *= reduction; | ||
// during takeoff we want to prioritise roll control over | ||
// pitch. Apply a reduction in pitch demand if our roll is | ||
// significantly off. The aim of this change is to | ||
// increase the robustness of hand launches, particularly | ||
// in cross-winds. If we start to roll over then we reduce | ||
// pitch demand until the roll recovers | ||
float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90)); | ||
float reduction = sq(cosf(roll_error_rad)); | ||
nav_pitch_cd *= reduction; | ||
|
||
if (nav_pitch_cd < pitch_min_cd) { | ||
pitch_min_cd = nav_pitch_cd; | ||
} | ||
} | ||
// Notify TECS about the external pitch setting, for the next iteration. | ||
TECS_controller.set_pitch_min(0.01f*pitch_min_cd); | ||
if (pitch_clipped_max) {TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This was VERY old code and didn't work as advertized. I hope it's at least easier to read now.
ArduPlane/takeoff.cpp
Outdated
// 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. | ||
if (!use_throttle_range || !ahrs.using_airspeed_sensor() || below_lvl_alt || use_max_throttle) { // Traditional takeoff throttle limit. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This allows AUTO mode to have access to TKOFF_LVL_ALT
logic.
I know it's a bit of AUTO mode reaching into TAKEOFF for data, but it's not the first time and I intend to address this in a future PR.
libraries/AP_TECS/AP_TECS.cpp
Outdated
@@ -1067,6 +1064,9 @@ void AP_TECS::_update_pitch(void) | |||
_pitch_dem = _last_pitch_dem - ptchRateIncr; | |||
} | |||
|
|||
// Constrain pitch demand | |||
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); | |||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Previously external pitch limits were applied before vertical acceleration. This would lead to the resulting pitch demand to exceed the external limits.
While this means that the final pitch demand might violate vertical acceleration, I feel it's more important to respect the external limits.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@priseborough are you ok with this re-ordering?
|
||
if (!_flag_throttle_forced) { | ||
// Calculate the takeoff target height offset before _hgt_dem_in_raw gets reset below. | ||
// Prevent the offset from becoming negative. | ||
_post_TO_hgt_offset = MAX(MIN(_climb_rate_limit * _hgt_dem_tconst, _hgt_dem_in_raw - hgt_afe), 0); | ||
} else { | ||
// If throttle is externally forced, this mechanism of adding energy is unnecessary. | ||
_post_TO_hgt_offset = 0; | ||
} | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The offset is now a little smarter:
If TECS is externally overridden, there's little point in having it.
In normal operation, we don't want to have the offset bring the demanded height above the raw target, as it leads to altitude overshoots in general. And we also don't want it to become negative.
Here;s a small opening to a potential bug. This offset is the only mechanism for adding throttle (energy) during FlightStage::TAKOFF
. As it becomes zero as the altitude error becomes zero, throttle would also become zero and could lead to the altitude error never actually closing.
TKOFF_THR_MIN
(default=0.6) is necessary to ensure that we will reach the altitude.
bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING; | ||
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); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
use_throttle_range
was a bug here on my part. When TKOFF_OPTIONS=1
is irrelevant to using TKOFF_THR_MIN
.
Granted, this section would be better if it was identical to how TKOFF_THR_MAX
is used above.
9bf7812
to
968fc1d
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think there should be only one call into TECS to give the limits for the next loop, not two calls giving different limits
This is me thinking out loud and writing down my thought process: In the last dev call one of the review points was that TECS throttle limits should be set (through the Currently there are 3 places in the codebase that make decisions on the throttle limits and 2 places where the throttle is actually written.
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
Regarding which piece of code enacts which piece of logic: TECS logic takes into account:
What I think I'll do is:
|
I've pushed another pair of commits to address the review comments in the last Dev Call:
|
I went over @peterbarker 's flight test logs. Thanks Peter! |
this is failing a flapping copter fence test that I thin @andypiper has a PR to disable |
Ah, thanks for mentioning it. It did pass on my PC locally, but failing twice in a row in CI server got me a bit worried. |
bf21342
to
81422a2
Compare
81422a2
to
1c8de0f
Compare
I'm concerned that the glider-pull-up stuff is breaking in here... |
Henry was testing with Andrew yesterday and he told me a couple of days ago that might be an issue with the recent pullup PR itself; so I didn't pay much attention. |
Tridge was happy to merge with only GliderPullup fails....he knows that test has bugs....just hasn't gotten around to fixing it yet George, Tridge was going to merge until you pushed again....he and I went over the previous pushes vs my testing yesterday and was happy to merge...but then you pushed again, with what changes, I don't know...we need to stick a pin in this or we will revert your previous takeoff changes so 4.6 can roll out...I don't look forward to asking Tridge again to figure out if the changes you last pushed need retesting! |
@Hwurzburg we do need to work out why GliderPullup is failing |
4c48f09
to
8037208
Compare
- Refactor and split set_pitch_max_limit method. - New _update_pitch_limits to encapsulate all relevant functionality. - Automatically reset if pitch and throttle are overriden. - nullified TAKEOFF alt_dem offset on external throttle. - Simplify use of TKOFF_THR_MIN. - Prevent takeoff altitude overshoot by capping the altitude setpoint offset. - Move pitch limits after vertical acceleration limitation.
- TAKEOFF and AUTO flight modes now should have identical takeoff - Prevent behaviour switching past climb altitude in TAKEOFF mode. - Refactor set_pitch_min/max methods. Max was already there, now renamed. Min is newly introduced. behaviour. - Remove enforcement of min takeoff throttle logic from servos.cpp. It is now handled only by takeoff.cpp. - Take TKOFF_LVL_ALT into consideration in AUTO as well. - Fixed pitch setpoint when TKOFF_ROTATE_SPD>0. - Roll navigation in mode TAKEOFF during climb should now work again. - Now the TAKEOFF loiter waypoint is set by the bearing of the aircraft while on TKOFF_LVL_ALT, as in the last stable release, instead of TKOFF_ALT. - Using TRIM_THROTTLE in takeoffs, when TKOFF_THR_MIN==0
- Also added a ground rolling takeoff test. - Rebased conflict resolution originating from ArduPilot#28030
8037208
to
3c6a048
Compare
Would be nice to update the documentation to note that the pitch-max should be negative on a glider like this, @Hwurzburg |
@Georacer has a wiki PR for this and it should be incorporated into his PR |
@Hwurzburg what Peter is talking about is something different: We were discussing in the dev call today that pure gliders (no motor) should probably be set with a maximum pitch angle that is close to zero (or negative), such that they can't pull their nose up and stall. |
okay will add to TECS page where that param is discussed |
I tested this today. Definitely much improved "overshoot" behavior on auto takeoff. It no longer hits the top of the climb and then dives back down like it was doing before this. |
@timtuxworth that's great to hear! |
This PR fixes TECS oscillations that would occur once mode TAKEOFF would reach
TKOFF_ALT
.Additionally, it harmonizes the takeoff behaviour between modes AUTO and TAKEOFF.
Finally, it fixes a few bugs.
Many thanks to @Hwurzburg for sharing code for fixing the pitch setpoint.
Notable behaviour changes
TKOFF_LVL_ALT
now also affects AUTO mode.Notable code changes
TECS::set_pitch_max_limit()
is now split intoTECS::set_pitch_max()
andTECS::set_pitch_min()
. Its usage inquadplane.cpp
has been modified accordingly._post_TO_hgt_offset
will now never pull height demand above the target altitude.mode_takeoff.cpp
is now prevented from switching behaviours from pastTKOFF_ALT
-2m back to climb behaviour. This would cause behaviour switching if the plane would drop slightly lower than 2m fromTKOFF_ALT
.TECS_PITCH_MIN
to match description.servos.cpp
now doesn't try to enforce all of the takeoff throttle logic combinations. OnlyTKOFF_THR_MAX
andTKOFF_THR_MIN
.Tests
The following were tested in autotests:
ARSPD_USE
andTKOFF_OPTIONS
. ✅TKOFF_ROTATE_SPD
is nonzero, pitch demand will be 5deg before the rotation speed and will climb up to the minimum takeoff pitch for groundspeed equal to cruise airspeed. ✅The following were tested in manual SITL:
TKOFF_ROTATE_SPD
is nonzero, pitch demand will be 5deg before the rotation speed and will climb up to the minimum takeoff pitch for groundspeed equal to cruise airspeed. ✅Known issues
Future work
TKOFF_LVL_ALT
a higher-level parameter, as it is now used also by AUTO mode, not just TAKEOFF.