Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Peter Barker 2021-08-11 22:11:54 +10:00 committed by Tom Pittenger
parent dba40481c6
commit 73bad9fd2d
4 changed files with 28 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -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

21
libraries/AP_AHRS/AP_AHRS.h
View File

@ -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

10
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -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;

11
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -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);