Skip to content

Commit 5c9f41c

Browse files
committed
Plane: Added check for takeoff level-off timeout
When an airspeed sensor is not used, during a takeoff, the pitch angle is asymptotically driven to 0 as the takeoff altitude is approached. Some airplanes will then stop climbing and fail to reach altitude. To prevent an indefinite wait for the takeoff altitude to be reached, a dedicated level-off timeout has been introduced.
1 parent 657680a commit 5c9f41c

File tree

4 files changed

+30
-4
lines changed

4 files changed

+30
-4
lines changed

ArduPlane/Plane.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -453,7 +453,7 @@ class Plane : public AP_Vehicle {
453453
float throttle_lim_max;
454454
float throttle_lim_min;
455455
uint32_t throttle_max_timer_ms;
456-
// Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
456+
uint32_t level_off_start_time_ms;
457457
} takeoff_state;
458458

459459
// ground steering controller state
@@ -1147,6 +1147,7 @@ class Plane : public AP_Vehicle {
11471147
int16_t get_takeoff_pitch_min_cd(void);
11481148
void landing_gear_update(void);
11491149
bool check_takeoff_timeout(void);
1150+
bool check_takeoff_timeout_level_off(void);
11501151

11511152
// avoidance_adsb.cpp
11521153
void avoidance_adsb_update(void);

ArduPlane/commands_logic.cpp

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -587,7 +587,10 @@ bool Plane::verify_takeoff()
587587

588588
// see if we have reached takeoff altitude
589589
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
590-
if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
590+
if (
591+
relative_alt_cm > auto_state.takeoff_altitude_rel_cm || // altitude reached
592+
plane.check_takeoff_timeout_level_off() // pitch level-off maneuver has timed out
593+
) {
591594
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
592595
(double)(relative_alt_cm*0.01f));
593596
steer_state.hold_course_cd = -1;

ArduPlane/mode_takeoff.cpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,7 @@ void ModeTakeoff::update()
119119
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
120120
alt, dist, direction);
121121
plane.takeoff_state.start_time_ms = millis();
122+
plane.takeoff_state.level_off_start_time_ms = 0;
122123
plane.takeoff_state.throttle_max_timer_ms = millis();
123124
takeoff_mode_setup = true;
124125
plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
@@ -157,6 +158,12 @@ void ModeTakeoff::update()
157158
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
158159
}
159160

161+
// If we have timed-out on the attempt to close the final few meters
162+
// during pitch level-off, continue to NORMAL flight stage.
163+
if (plane.check_takeoff_timeout_level_off()) {
164+
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
165+
}
166+
160167
if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
161168
//below TKOFF_ALT
162169
plane.takeoff_calc_roll();

ArduPlane/takeoff.cpp

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void)
121121
takeoff_state.launchTimerStarted = false;
122122
takeoff_state.last_tkoff_arm_time = 0;
123123
takeoff_state.start_time_ms = now;
124+
takeoff_state.level_off_start_time_ms = 0;
124125
takeoff_state.throttle_max_timer_ms = now;
125126
steer_state.locked_course_err = 0; // use current heading without any error offset
126127
return true;
@@ -316,6 +317,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void)
316317
// make a note of that altitude to use it as a start height for scaling
317318
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100));
318319
auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
320+
takeoff_state.level_off_start_time_ms = AP_HAL::millis();
319321
}
320322
}
321323
}
@@ -376,9 +378,8 @@ void Plane::landing_gear_update(void)
376378
#endif
377379

378380
/*
379-
check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout
381+
check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred
380382
*/
381-
382383
bool Plane::check_takeoff_timeout(void)
383384
{
384385
if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) {
@@ -400,3 +401,17 @@ bool Plane::check_takeoff_timeout(void)
400401
return false;
401402
}
402403

404+
/*
405+
check if the pitch level-off time has expired; returns true if timeout has occurred
406+
*/
407+
bool Plane::check_takeoff_timeout_level_off(void)
408+
{
409+
if (takeoff_state.level_off_start_time_ms > 0) {
410+
// A takeoff is in progress.
411+
uint32_t now = AP_HAL::millis();
412+
if ((now - takeoff_state.level_off_start_time_ms) > (uint32_t)(1000U * g.takeoff_pitch_limit_reduction_sec)) {
413+
return true;
414+
}
415+
}
416+
return false;
417+
}

0 commit comments

Comments
 (0)