From a4f71e5946706e3976d872df457b0aa3fec8223a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 2 Jan 2015 17:09:14 +0900 Subject: [PATCH] Copter: replace inav's position_ok with get_filter_status --- ArduCopter/control_auto.pde | 2 +- ArduCopter/control_circle.pde | 2 +- ArduCopter/control_loiter.pde | 2 +- ArduCopter/control_poshold.pde | 2 +- ArduCopter/control_rtl.pde | 2 +- ArduCopter/navigation.pde | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 9c90aec8fe..5b1857a7bc 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -20,7 +20,7 @@ // auto_init - initialise auto controller static bool auto_init(bool ignore_checks) { - if ((GPS_ok() && inertial_nav.position_ok() && mission.num_commands() > 1) || ignore_checks) { + if ((GPS_ok() && inertial_nav.get_filter_status().flags.horiz_pos_abs && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; // stop ROI from carrying over from previous runs of the mission diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index f336c3f3ea..c9c1e5e244 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -7,7 +7,7 @@ // circle_init - initialise circle controller flight mode static bool circle_init(bool ignore_checks) { - if ((GPS_ok() && inertial_nav.position_ok()) || ignore_checks) { + if ((GPS_ok() && inertial_nav.get_filter_status().flags.horiz_pos_abs) || ignore_checks) { circle_pilot_yaw_override = false; // initialize speeds and accelerations diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 572bb49228..7cf4d4b33c 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -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.position_ok()) { + if(!ap.auto_armed || !inertial_nav.get_filter_status().flags.horiz_pos_abs) { wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index 457dc0453e..88cbbe91fb 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -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.position_ok()) { + if(!ap.auto_armed || !inertial_nav.get_filter_status().flags.horiz_pos_abs) { wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index eee99880a5..4ac396dec4 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -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.position_ok()) { + if(!ap.auto_armed || !inertial_nav.get_filter_status().flags.horiz_pos_abs) { attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 1d700d6576..ec0da5b26d 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -17,7 +17,7 @@ static void run_nav_updates(void) // calc_position - get lat and lon positions from inertial nav library static void calc_position(){ - if( inertial_nav.position_ok() ) { + if (inertial_nav.get_filter_status().flags.horiz_pos_abs) { // pull position from interial nav library current_loc.lng = inertial_nav.get_longitude(); current_loc.lat = inertial_nav.get_latitude();