From 199dc3454d4a6071548c7639e3ea62b17fd4117e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 2 Jan 2015 20:43:50 +0900 Subject: [PATCH] Copter: replace GPS_ok with position_ok position_ok uses the EKF's filter status if the EKF is being used otherwise it falls back to the GPS based checks used by inertial nav --- ArduCopter/control_auto.pde | 4 ++-- ArduCopter/control_circle.pde | 2 +- ArduCopter/control_drift.pde | 2 +- ArduCopter/control_guided.pde | 2 +- ArduCopter/control_land.pde | 2 +- ArduCopter/control_loiter.pde | 2 +- ArduCopter/control_poshold.pde | 2 +- ArduCopter/control_rtl.pde | 2 +- ArduCopter/heli.pde | 2 +- ArduCopter/motors.pde | 2 +- ArduCopter/navigation.pde | 2 +- ArduCopter/system.pde | 18 ++++++++++-------- 12 files changed, 22 insertions(+), 20 deletions(-) diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 5b1857a7bc..0666701f6d 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.get_filter_status().flags.horiz_pos_abs && mission.num_commands() > 1) || ignore_checks) { + if ((position_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 @@ -430,7 +430,7 @@ void auto_nav_guided_run() bool auto_loiter_start() { // return failure if GPS is bad - if (!GPS_ok()) { + if (!position_ok()) { return false; } auto_mode = Auto_Loiter; diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index c9c1e5e244..c4fad8fadd 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.get_filter_status().flags.horiz_pos_abs) || ignore_checks) { + if ((position_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_drift.pde b/ArduCopter/control_drift.pde index 3217e3e46b..a5c1c7f548 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -29,7 +29,7 @@ // drift_init - initialise drift controller static bool drift_init(bool ignore_checks) { - if (GPS_ok() || ignore_checks) { + if (position_ok() || ignore_checks) { return true; }else{ return false; diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 6e0598733b..f9294116fe 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -26,7 +26,7 @@ struct Guided_Limit { // guided_init - initialise guided controller static bool guided_init(bool ignore_checks) { - if (GPS_ok() || ignore_checks) { + if (position_ok() || ignore_checks) { // initialise yaw set_auto_yaw_mode(get_default_auto_yaw_mode(false)); // start in position control mode diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 9fabe14534..34813131d3 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -9,7 +9,7 @@ static bool land_pause; static bool land_init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do - land_with_gps = GPS_ok(); + land_with_gps = position_ok(); if (land_with_gps) { // set target to stopping point Vector3f stopping_point; diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 7cf4d4b33c..3963f33fbc 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -7,7 +7,7 @@ // loiter_init - initialise loiter controller static bool loiter_init(bool ignore_checks) { - if (GPS_ok() || ignore_checks) { + if (position_ok() || ignore_checks) { // set target to current position wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index 88cbbe91fb..e7e5a40435 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -97,7 +97,7 @@ static struct { static bool poshold_init(bool ignore_checks) { // fail to initialise PosHold mode if no GPS lock - if (!GPS_ok() && !ignore_checks) { + if (!position_ok() && !ignore_checks) { return false; } diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 4ac396dec4..6032f0d5a9 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -10,7 +10,7 @@ // rtl_init - initialise rtl controller static bool rtl_init(bool ignore_checks) { - if (GPS_ok() || ignore_checks) { + if (position_ok() || ignore_checks) { rtl_climb_start(); return true; }else{ diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index b823a80434..d29663031b 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -51,7 +51,7 @@ static void check_dynamic_flight(void) bool moving = false; // with GPS lock use inertial nav to determine if we are moving - if (GPS_ok()) { + if (position_ok()) { // get horizontal velocity float velocity = inertial_nav.get_velocity_xy(); moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN); diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index b37e60e49c..24c89ab0d5 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -508,7 +508,7 @@ static bool pre_arm_gps_checks(bool display_failure) } // ensure GPS is ok - if (!GPS_ok()) { + if (!position_ok()) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix")); } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index ec0da5b26d..dfef3ef3c8 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -64,7 +64,7 @@ static void calc_home_distance_and_bearing() Vector3f curr = inertial_nav.get_position(); // calculate home distance and bearing - if (GPS_ok()) { + if (position_ok()) { home_distance = pythagorous2(curr.x, curr.y); home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0)); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 46fe29c6a8..4f0c489fa2 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -335,15 +335,17 @@ static void startup_ground(bool force_gyro_cal) set_land_complete_maybe(true); } -// returns true if the GPS is ok and home position is set -static bool GPS_ok() +// position_ok - returns true if the horizontal absolute position is ok and home position is set +static bool position_ok() { - if (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D && - !gps_glitch.glitching() && !failsafe.gps && - !ekf_check_state.bad_compass && !failsafe.ekf) { - return true; - }else{ - return false; + if (ahrs.have_inertial_nav()) { + // with EKF use filter status and ekf check + return (inertial_nav.get_filter_status().flags.horiz_pos_abs && !failsafe.ekf); + } else { + // with interial nav use GPS based checks + return (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D && + !gps_glitch.glitching() && !failsafe.gps && + !ekf_check_state.bad_compass && !failsafe.ekf); } }