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
This commit is contained in:
Randy Mackay 2015-01-02 20:43:50 +09:00
parent a4f71e5946
commit 199dc3454d
12 changed files with 22 additions and 20 deletions

View File

@ -20,7 +20,7 @@
// auto_init - initialise auto controller // auto_init - initialise auto controller
static bool auto_init(bool ignore_checks) 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; auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission // stop ROI from carrying over from previous runs of the mission
@ -430,7 +430,7 @@ void auto_nav_guided_run()
bool auto_loiter_start() bool auto_loiter_start()
{ {
// return failure if GPS is bad // return failure if GPS is bad
if (!GPS_ok()) { if (!position_ok()) {
return false; return false;
} }
auto_mode = Auto_Loiter; auto_mode = Auto_Loiter;

View File

@ -7,7 +7,7 @@
// circle_init - initialise circle controller flight mode // circle_init - initialise circle controller flight mode
static bool circle_init(bool ignore_checks) 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; circle_pilot_yaw_override = false;
// initialize speeds and accelerations // initialize speeds and accelerations

View File

@ -29,7 +29,7 @@
// drift_init - initialise drift controller // drift_init - initialise drift controller
static bool drift_init(bool ignore_checks) static bool drift_init(bool ignore_checks)
{ {
if (GPS_ok() || ignore_checks) { if (position_ok() || ignore_checks) {
return true; return true;
}else{ }else{
return false; return false;

View File

@ -26,7 +26,7 @@ struct Guided_Limit {
// guided_init - initialise guided controller // guided_init - initialise guided controller
static bool guided_init(bool ignore_checks) static bool guided_init(bool ignore_checks)
{ {
if (GPS_ok() || ignore_checks) { if (position_ok() || ignore_checks) {
// initialise yaw // initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode // start in position control mode

View File

@ -9,7 +9,7 @@ static bool land_pause;
static bool land_init(bool ignore_checks) static bool land_init(bool ignore_checks)
{ {
// check if we have GPS and decide which LAND we're going to do // 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) { if (land_with_gps) {
// set target to stopping point // set target to stopping point
Vector3f stopping_point; Vector3f stopping_point;

View File

@ -7,7 +7,7 @@
// loiter_init - initialise loiter controller // loiter_init - initialise loiter controller
static bool loiter_init(bool ignore_checks) static bool loiter_init(bool ignore_checks)
{ {
if (GPS_ok() || ignore_checks) { if (position_ok() || ignore_checks) {
// set target to current position // set target to current position
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();

View File

@ -97,7 +97,7 @@ static struct {
static bool poshold_init(bool ignore_checks) static bool poshold_init(bool ignore_checks)
{ {
// fail to initialise PosHold mode if no GPS lock // fail to initialise PosHold mode if no GPS lock
if (!GPS_ok() && !ignore_checks) { if (!position_ok() && !ignore_checks) {
return false; return false;
} }

View File

@ -10,7 +10,7 @@
// rtl_init - initialise rtl controller // rtl_init - initialise rtl controller
static bool rtl_init(bool ignore_checks) static bool rtl_init(bool ignore_checks)
{ {
if (GPS_ok() || ignore_checks) { if (position_ok() || ignore_checks) {
rtl_climb_start(); rtl_climb_start();
return true; return true;
}else{ }else{

View File

@ -51,7 +51,7 @@ static void check_dynamic_flight(void)
bool moving = false; bool moving = false;
// with GPS lock use inertial nav to determine if we are moving // with GPS lock use inertial nav to determine if we are moving
if (GPS_ok()) { if (position_ok()) {
// get horizontal velocity // get horizontal velocity
float velocity = inertial_nav.get_velocity_xy(); float velocity = inertial_nav.get_velocity_xy();
moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN); moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);

View File

@ -508,7 +508,7 @@ static bool pre_arm_gps_checks(bool display_failure)
} }
// ensure GPS is ok // ensure GPS is ok
if (!GPS_ok()) { if (!position_ok()) {
if (display_failure) { if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
} }

View File

@ -64,7 +64,7 @@ static void calc_home_distance_and_bearing()
Vector3f curr = inertial_nav.get_position(); Vector3f curr = inertial_nav.get_position();
// calculate home distance and bearing // calculate home distance and bearing
if (GPS_ok()) { if (position_ok()) {
home_distance = pythagorous2(curr.x, curr.y); home_distance = pythagorous2(curr.x, curr.y);
home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0)); home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));

View File

@ -335,15 +335,17 @@ static void startup_ground(bool force_gyro_cal)
set_land_complete_maybe(true); set_land_complete_maybe(true);
} }
// returns true if the GPS is ok and home position is set // position_ok - returns true if the horizontal absolute position is ok and home position is set
static bool GPS_ok() static bool position_ok()
{ {
if (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D && if (ahrs.have_inertial_nav()) {
!gps_glitch.glitching() && !failsafe.gps && // with EKF use filter status and ekf check
!ekf_check_state.bad_compass && !failsafe.ekf) { return (inertial_nav.get_filter_status().flags.horiz_pos_abs && !failsafe.ekf);
return true; } else {
}else{ // with interial nav use GPS based checks
return false; 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);
} }
} }