5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00

Copter: tighten auto_takeoff_complete checks

This commit is contained in:
Leonard Hall 2022-02-23 15:25:35 +10:30 committed by Andrew Tridgell
parent 3d8a6533d9
commit bcddd0b949

View File

@ -180,7 +180,7 @@ void Mode::auto_takeoff_run()
// handle takeoff completion
bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm - auto_take_off_start_alt_cm) * 0.90);
bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.5;
bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.1;
auto_takeoff_complete = reached_altitude && reached_climb_rate;
// calculate completion for location in case it is needed for a smooth transition to wp_nav