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.

Sign up for GitHub

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

Jump to bottom

Plane: Fix level-off and throttle without airspeed sensor #28776

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+9 −3
Open

Plane: Fix level-off and throttle without airspeed sensor #28776

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -454,6 +454,9 @@ class Plane : public AP_Vehicle {
float throttle_lim_min;
uint32_t throttle_max_timer_ms;
// Good candidate for keeping the initial time for TKOFF_THR_MAX_T.

bool in_level_off_phase;
// The Flag to indicate the last phase of takeoff, the pitch level-off to neutral.
} takeoff_state;

// ground steering controller state
Expand Down
9 changes: 6 additions & 3 deletions ArduPlane/takeoff.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,7 +208,7 @@ void Plane::takeoff_calc_pitch(void)
bool pitch_clipped_max = false;

// If we're using an airspeed sensor, we consult TECS.
if (ahrs.using_airspeed_sensor()) {
if (ahrs.using_airspeed_sensor() || (takeoff_state.in_level_off_phase == true)) {
calc_nav_pitch();
// At any rate, we don't want to go lower than the minimum pitch bound.
if (nav_pitch_cd < pitch_min_cd) {
Expand Down Expand Up @@ -279,7 +279,6 @@ void Plane::takeoff_calc_throttle() {
// Set the minimum throttle limit.
const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
if (!use_throttle_range // We don't want to employ a throttle range.
|| !ahrs.using_airspeed_sensor() // We don't have an airspeed sensor.
|| below_lvl_alt // We are below TKOFF_LVL_ALT.
) { // Traditional takeoff throttle limit.
takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max;
Expand Down Expand Up @@ -309,13 +308,17 @@ int16_t Plane::get_takeoff_pitch_min_cd(void)

// are we entering the region where we want to start levelling off before we reach takeoff alt?
if (auto_state.sink_rate < -0.1f) {
float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate);
float sec_to_target = (remaining_height_to_target_cm * 0.01f * 2.0f) / MIN(TECS_controller.get_max_climbrate(), -auto_state.sink_rate);
Copy link
Contributor

@Georacer Georacer Dec 1, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

The decrease of pitch is not linear; that much we've seen from the SITL tests.
But in any case, can you provide some justification for this formula?

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 make the case that the scalar creates the linear dependency between remaining height and pitch.
Integrating A*X results in (A/2)*X² and that's where the factor 2 comes from.
Setting both equal results in X=1/2 for the gained altitude.

The MIN() is a sanity check to limit climb-rate to the max value known to TECS.
It is not dependent on angle but on the power to weight ratio.

I don't know what happens in 4.5.7 but there it is not linear.

/* A linear decrease of pitch at level-off is also a linear decrease of climb-rate, thus the height gained is only half.
* Additionally, the climb-rate is limited by TECS value to keep external factors like wind influence at bay.
*/
if (sec_to_target > 0 &&
relative_alt_cm >= 1000 &&
sec_to_target <= g.takeoff_pitch_limit_reduction_sec) {
// make a note of that altitude to use it as a start height for scaling
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100));
auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
takeoff_state.in_level_off_phase = true;
}
}
}
Expand Down
Loading