From 73bad9fd2d780667d602dfab5a6cfc5eeb7bdf01 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 11 Aug 2021 22:11:54 +1000 Subject: [PATCH] AP_AHRS: move fly_forward flag to front end This same flag is requested by the DAL so AN_NavEKF2 and AP_NavEKF3 can use it in their estimates - so AP_AHRS_DCM accessing it via the same mechanism is not out-of-shape --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- libraries/AP_AHRS/AP_AHRS.h | 21 +++++++++++++++++++++ libraries/AP_AHRS/AP_AHRS_Backend.h | 10 ---------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 11 ++++++----- 4 files changed, 28 insertions(+), 16 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index c3aa992820..34cf418421 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1750,7 +1750,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const if (ret != EKFType::NONE && (_vehicle_class == AHRS_VEHICLE_FIXED_WING || _vehicle_class == AHRS_VEHICLE_GROUND) && - (_flags.fly_forward || !hal.util->get_soft_armed())) { + (fly_forward || !hal.util->get_soft_armed())) { bool should_use_gps = true; nav_filter_status filt_state; #if HAL_NAVEKF2_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 654e7d4e3e..e9a96dcfd4 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -401,6 +401,19 @@ public: return touchdown_expected; } + /* + * fly_forward is set by the vehicles to indicate the vehicle + * should generally be moving in the direction of its heading. + * It is an additional piece of information that the backends can + * use to provide additional and/or improved estimates. + */ + void set_fly_forward(bool b) { + fly_forward = b; + } + bool get_fly_forward(void) const { + return fly_forward; + } + protected: // optional view class AP_AHRS_View *_view; @@ -525,6 +538,14 @@ private: bool touchdown_expected; // true if the vehicle is in a state that touchdown might be expected. Ground effect may be in play. uint32_t touchdown_expected_start_ms; + /* + * fly_forward is set by the vehicles to indicate the vehicle + * should generally be moving in the direction of its heading. + * It is an additional piece of information that the backends can + * use to provide additional and/or improved estimates. + */ + bool fly_forward; // true if we can assume the vehicle will be flying forward on its X axis + #if HAL_NMEA_OUTPUT_ENABLED class AP_NMEA_Output* _nmea_out; #endif diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 185b2a44e5..bd9beb4065 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -58,15 +58,6 @@ public: // init sets up INS board orientation virtual void init(); - // Accessors - void set_fly_forward(bool b) { - _flags.fly_forward = b; - } - - bool get_fly_forward(void) const { - return _flags.fly_forward; - } - AHRS_VehicleClass get_vehicle_class(void) const { return _vehicle_class; } @@ -522,7 +513,6 @@ protected: // flags structure struct ahrs_flags { uint8_t have_initial_yaw : 1; // whether the yaw value has been intialised with a reference - uint8_t fly_forward : 1; // 1 if we can assume the aircraft will be flying forward on its X axis uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed) uint8_t wind_estimation : 1; // 1 if we should do wind estimation } _flags; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index d66358c666..5625eba632 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -429,7 +429,7 @@ bool AP_AHRS_DCM::use_compass(void) // no compass available return false; } - if (!_flags.fly_forward || !have_gps()) { + if (!AP::ahrs().get_fly_forward() || !have_gps()) { // we don't have any alterative to the compass return true; } @@ -507,7 +507,7 @@ AP_AHRS_DCM::drift_correction_yaw(void) // don't suddenly change yaw with a reset _gps_last_update = _gps.last_fix_time_ms(); } - } else if (_flags.fly_forward && have_gps()) { + } else if (AP::ahrs().get_fly_forward() && have_gps()) { /* we are using GPS for yaw */ @@ -669,6 +669,7 @@ AP_AHRS_DCM::drift_correction(float deltat) _ra_deltat += deltat; const AP_GPS &_gps = AP::gps(); + const bool fly_forward = AP::ahrs().get_fly_forward(); if (!have_gps() || _gps.status() < AP_GPS::GPS_OK_FIX_3D || @@ -761,7 +762,7 @@ AP_AHRS_DCM::drift_correction(float deltat) bool using_gps_corrections = false; float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS); - if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) { + if (_flags.correct_centrifugal && (_have_gps_lock || fly_forward)) { const float v_scale = gps_gain.get() * ra_scale; const Vector3f vdelta = (velocity - _last_velocity) * v_scale; GA_e += vdelta; @@ -899,7 +900,7 @@ AP_AHRS_DCM::drift_correction(float deltat) _omega_P *= 8; } - if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && + if (fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && _gps.ground_speed() < GPS_SPEED_MIN && _ins.get_accel().x >= 7 && pitch_sensor > -3000 && pitch_sensor < 3000) { @@ -1032,7 +1033,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const loc.relative_alt = 0; loc.terrain_alt = 0; loc.offset(_position_offset_north, _position_offset_east); - if (_flags.fly_forward && _have_position) { + if (AP::ahrs().get_fly_forward() && _have_position) { float gps_delay_sec = 0; gps.get_lag(gps_delay_sec); loc.offset_bearing(gps.ground_course(), gps.ground_speed() * gps_delay_sec);