mirror of https://github.com/ArduPilot/ardupilot
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:
parent
a4f71e5946
commit
199dc3454d
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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{
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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"));
|
||||
}
|
||||
|
|
|
@ -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));
|
||||
|
||||
|
|
|
@ -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;
|
||||
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 {
|
||||
return false;
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue