Browse Source

Copter: tighten auto_takeoff_complete checks

Copter-4.2
Leonard Hall 3 years ago committed by Randy Mackay
parent
commit
2aa3c0c478
  1. 2
      ArduCopter/takeoff.cpp

2
ArduCopter/takeoff.cpp

@ -180,7 +180,7 @@ void Mode::auto_takeoff_run() @@ -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

Loading…
Cancel
Save