mirror of https://github.com/ArduPilot/ardupilot
Copter: replace inav's position_ok with get_filter_status
This commit is contained in:
parent
c24e997fb2
commit
a4f71e5946
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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();
|
||||
|
|
Loading…
Reference in New Issue