Skip to content
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

Open
wants to merge 7 commits into
base: master
Choose a base branch
from

Conversation

Georacer
Copy link
Contributor

@Georacer Georacer commented Aug 5, 2024

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

  1. Mode TAKEOFF and AUTO now behave identically during a takeoff.
  2. TKOFF_LVL_ALT now also affects AUTO mode.
  3. TAKEOFF mode now respects the level-off angles, as it approaches altitude. There should now be significantly less overshoot.
  4. The behaviour of TKOFF_ROTATION_SPD now works as advertized. It was inactive before.

Notable code changes

  1. Mode TAKEOFF now spends all of the climb in FlightStage::TAKEOFF, just like AUTO mode did.
  2. TECS::set_pitch_max_limit() is now split into TECS::set_pitch_max() and TECS::set_pitch_min(). Its usage in quadplane.cpp has been modified accordingly.
  3. TECS pitch limits are now applied at the end of the calculation. Before, vertical acceleration limitations were applied last.
  4. TECS: _post_TO_hgt_offset will now never pull height demand above the target altitude.
  5. mode_takeoff.cpp is now prevented from switching behaviours from past TKOFF_ALT-2m back to climb behaviour. This would cause behaviour switching if the plane would drop slightly lower than 2m from TKOFF_ALT.
  6. There are many occasions were TECS is overridden both in pitch and throttle. When that happens, TECS is now effectively inactive and will reset itself.
  7. Changed behaviour of TECS_PITCH_MIN to match description.
  8. servos.cpp now doesn't try to enforce all of the takeoff throttle logic combinations. Only TKOFF_THR_MAX and TKOFF_THR_MIN.

Tests

The following were tested in autotests:

  1. Altitude and pitch no longer oscillation upon reaching the takeoff altitude. ✅
  2. TAKEOFF and AUTO takeoffs now behave exactly the same in all combinations of ARSPD_USE and TKOFF_OPTIONS. ✅
  3. The level-off of minimum pitch demand now works in TAKEOFF. ✅
  4. When 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:

  1. Altitude and pitch no longer oscillation upon reaching the takeoff altitude. ✅
  2. The level-off of minimum pitch demand now works in TAKEOFF. ✅
  3. When 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

  1. Ground speed is used in some airspeed comparisons.
  2. If AUTO is engaged while in flight and has a NAV_TAKEOFF point, it will force the pitch to positive while it gets processed. This lasts for a few cycles.
  3. The SITL plane doesn't regulate pitch well when it's flying at full throttle and fast. Nothing to do with this PR, just an observation.

Future work

  1. I noticed that if you load a mission before you get a GNSS fix, the NAV_TAKEOFF waypoint (i.e. next_WP_loc) will be set at coordinates at the home coordinates and altitude. This can break some takeoff logic in AUTO mode.
  2. Render TKOFF_LVL_ALT a higher-level parameter, as it is now used also by AUTO mode, not just TAKEOFF.

@Georacer Georacer requested a review from Hwurzburg August 5, 2024 15:45
@Georacer
Copy link
Contributor Author

Georacer commented Aug 5, 2024

This is the new behaviour of arming in mode TAKEOFF in stock SITL Edit: see update below:
image

@Georacer Georacer force-pushed the pr/tecs_improvements branch 3 times, most recently from 9883163 to 33e81d9 Compare August 7, 2024 19:51
@@ -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
Copy link
Contributor Author

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().

Comment on lines 206 to 234
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;
Copy link
Contributor Author

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);
Copy link
Contributor Author

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.

_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);
Copy link
Contributor Author

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) {
Copy link
Contributor Author

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.

Comment on lines -182 to +242
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);}
Copy link
Contributor Author

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.

// 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.
Copy link
Contributor Author

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.

@@ -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);

Copy link
Contributor Author

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.

Copy link
Contributor

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?

Comment on lines +1161 to +1168

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;
}

Copy link
Contributor Author

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);
Copy link
Contributor Author

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.

@Georacer Georacer added WikiNeeded needs wiki update DevCallEU labels Aug 7, 2024
@Georacer
Copy link
Contributor Author

Georacer commented Aug 7, 2024

Selected responses:

ARSPD_USE=1, TKOFF_OPTIONS=0:
image

ARSPD_USE=1, TKOFF_OPTIONS=1:
image

ARPSD_USE=0
image

@Georacer Georacer changed the title TECS improvements Plane: Takeoff improvements Aug 8, 2024
Copy link
Contributor

@tridge tridge left a 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

ArduPlane/mode.h Outdated Show resolved Hide resolved
@tridge tridge removed the DevCallEU label Aug 14, 2024
@Georacer
Copy link
Contributor Author

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 TECS.set_throttle_min/max() in one place in the codebase.
The current situation where multiple places invoke this and affect the limits is confusing.

Currently there are 3 places in the codebase that make decisions on the throttle limits and 2 places where the throttle is actually written.

  • Plane::update_control_mode() runs as a FAST_TASK, on every loop, calling the update() of each throttle-controlling flight mode.
    • This invokes eventually Plane::takeoff_calc_throttle(), which writes throttle limits to TECS,
    • then asks TECS for the throttle output in `Plane::calc_throttle(),
    • then writes on
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
  • servos.cpp:set_servos() runs as a FAST_TASK, on every loop.
    • These will overwrite the SRV_Channel::k_throttle values (assuming it runs after update_control_mode(), as it has lower priority).
    • They will also set the new, final limits to TECS.
  • TECS control updates occur under the update_alt() task, at 10Hz.

Regarding which piece of code enacts which piece of logic:

TECS logic takes into account:

  • THR_MAX/MIN
  • TKOFF_THR_MAX/MIN
  • TKOFF_THR_MAX_T
  • FlightStage, as a dependency of the above.
  • landing.is_flaring() to set min to 0.

Plane::takeoff_calc_throttle() logic takes into account:

  • TKOFF_THR_MAX/MIN
  • TKOFF_OPTIONS

Plane::apply_throttle_limits() logic takes into account:

  • THR_MAX/MIN
  • TKOFF_THR_MAX/MIN
  • landing.is_flaring() to set min to 0.

What I think I'll do is:

  1. Move all usage of THR_MAX/MIN to Plane::apply_throttle_limits()
  2. Move all usage of landing.is_flaring() to apply_throttle_limits().
  3. Move all usage of TKOFF_THR_MAX/MIN, TKOFF_THR_MAX_T,TKOFF_OPTIONS to takeoff_calc_throttle(). After all the takeoff part of the codebase is the one that should logically apply the takeoff-related parameters.
  4. takeoff_calc_throttle() will calculate throttle limits and store them in _tkoff_thr_max/min. These will also be retrievable via get_tkoff_thr_max/min. Care is needed that they will get restored when takeoff_calc_throttle() is no longer called.
  5. apply_throttle_limits() will bring in get_tkoff_thr_max/min() and take it into account towards calculating the limited_throttle. It will then set those limits to TECS. This will be the only place to do so, because it knows the final limits that are used.

@Georacer
Copy link
Contributor Author

I've pushed another pair of commits to address the review comments in the last Dev Call:
The throttle limits logic has been refactored such that:

  • TECS is now completely agnostic of the throttle limits. On every control iteration servos.cpp:apply_throttle_limits() is expected to set them.
  • servos.cpp:apply_throttle_limits() does not take into account takeoff-related throttle parameters. They have all been moved to takeoff.cpp. apply_throttle_limits() is expected to query takeoff.cpp for the limts, where applicable.
  • takeoff_calc_throttle() now writes the takeoff related throttle limits into takeoff_state new struct members.

@Georacer
Copy link
Contributor Author

I went over @peterbarker 's flight test logs.
All went as expected: he had set up ARSPD_USE=0 and TKOFF_OPTIONS=0, so the aircraft climbed at full throttle until TKOFF_ALT or until TKOFF_DIST
It followed the takeoff profile desired pitch exactly, except for the intervals where the pilot mixed pitch input, as expected.

Thanks Peter!

@Hwurzburg
Copy link
Collaborator

this is failing a flapping copter fence test that I thin @andypiper has a PR to disable

@Georacer
Copy link
Contributor Author

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.

ArduPlane/mode_takeoff.cpp Outdated Show resolved Hide resolved
- 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
Also added a ground rolling takeoff test.
- Fixed lack of heading control during TAKEOFF climb.
- Also 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.
This is related with the recent change of the TKOFF_THR_MIN default
value from 60 to 0.
@@ -429,6 +431,10 @@ class AP_TECS {

// need to reset on next loop
bool _need_reset;
// Flag if someone else drives pitch externally.
bool _flag_pitch_forced;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i'm not keen on the reset code as a special case for under 1 degree pitch range, it implies we have some bad behaviour at narrow pitch ranges, eg. 1.5 degrees. If we don't have any bad behaviour then why do we need the special case?

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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this doesn't look right, this is using fixed pitch after rotate and before cruise speed. That doesn't sound right at all, we won't be able to rotate

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we've lost the case of with an airspeed sensor and doing rotating rolling takeoff

@MattKear MattKear removed the DevCallEU label Sep 4, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants