mirror of https://github.com/ArduPilot/ardupilot
Copter: fix to RTL, PosHold and Loiter's use of filter status
This commit is contained in:
parent
cc52bbbffb
commit
4185d17915
|
@ -33,7 +33,7 @@ static void loiter_run()
|
|||
float target_climb_rate = 0;
|
||||
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed || !inertial_nav.get_filter_status().flags.horiz_pos_abs) {
|
||||
if(!ap.auto_armed || !ap.home_is_set) {
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
|
|
|
@ -154,7 +154,7 @@ static void poshold_run()
|
|||
const Vector3f& vel = inertial_nav.get_velocity();
|
||||
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed || !inertial_nav.get_filter_status().flags.horiz_pos_abs) {
|
||||
if(!ap.auto_armed || !ap.home_is_set) {
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
|
|
|
@ -261,7 +261,7 @@ static void rtl_descent_run()
|
|||
float target_yaw_rate = 0;
|
||||
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed || !inertial_nav.get_filter_status().flags.horiz_pos_abs) {
|
||||
if(!ap.auto_armed || !ap.home_is_set) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
|
|
Loading…
Reference in New Issue