Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: use GPS singleton

Browse Source
This commit is contained in:
Peter Barker 2017-12-02 10:50:46 +11:00 committed by Francisco Ferreira
parent ca4f25a78e
commit e15442355a
7 changed files with 30 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -143,10 +143,10 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const
{
if (airspeed_sensor_enabled()) {
*airspeed_ret = _airspeed->get_airspeed();
if (_wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
const float gnd_speed = _gps.ground_speed();
const float gnd_speed = AP::gps().ground_speed();
float true_airspeed = *airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max,
@ -193,7 +193,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
Vector2f gndVelGPS;
float airspeed;
const bool gotAirspeed = airspeed_estimate_true(&airspeed);
const bool gotGPS = (_gps.status() >= AP_GPS::GPS_OK_FIX_2D);
const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
if (gotAirspeed) {
const Vector3f wind = wind_estimate();
const Vector2f wind2d(wind.x, wind.y);
@ -203,8 +203,8 @@ Vector2f AP_AHRS::groundspeed_vector(void)
// Generate estimate of ground speed vector using GPS
if (gotGPS) {
const float cog = radians(_gps.ground_course_cd()*0.01f);
gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps.ground_speed();
const float cog = radians(AP::gps().ground_course_cd()*0.01f);
gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed();
}
// If both ADS and GPS data is available, apply a complementary filter
if (gotAirspeed && gotGPS) {

8
libraries/AP_AHRS/AP_AHRS.h
View File

@ -53,7 +53,7 @@ public:
friend class AP_AHRS_View;
// Constructor
AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro) :
roll(0.0f),
pitch(0.0f),
yaw(0.0f),
@ -68,7 +68,6 @@ public:
_compass_last_update(0),
_ins(ins),
_baro(baro),
_gps(gps),
_cos_roll(1.0f),
_cos_pitch(1.0f),
_cos_yaw(1.0f),
@ -201,10 +200,6 @@ public:
return _beacon;
}
const AP_GPS &get_gps() const {
return _gps;
}
const AP_InertialSensor &get_ins() const {
return _ins;
}
@ -628,7 +623,6 @@ protected:
// IMU under us without our noticing.
AP_InertialSensor &_ins;
AP_Baro &_baro;
const AP_GPS &_gps;
// a vector to capture the difference between the controller and body frames
AP_Vector3f _trim;

17
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -364,7 +364,7 @@ AP_AHRS_DCM::_yaw_gain(void) const
// return true if we have and should use GPS
bool AP_AHRS_DCM::have_gps(void) const
{
if (_gps.status() <= AP_GPS::NO_FIX || !_gps_use) {
if (AP::gps().status() <= AP_GPS::NO_FIX || !_gps_use) {
return false;
}
return true;
@ -395,7 +395,7 @@ bool AP_AHRS_DCM::use_compass(void)
// we don't have any alterative to the compass
return true;
}
if (_gps.ground_speed() < GPS_SPEED_MIN) {
if (AP::gps().ground_speed() < GPS_SPEED_MIN) {
// we are not going fast enough to use the GPS
return true;
}
@ -404,8 +404,8 @@ bool AP_AHRS_DCM::use_compass(void)
// degrees and the estimated wind speed is less than 80% of the
// ground speed, then switch to GPS navigation. This will help
// prevent flyaways with very bad compass offsets
const int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));
if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) {
const int32_t error = abs(wrap_180_cd(yaw_sensor - AP::gps().ground_course_cd()));
if (error > 4500 && _wind.length() < AP::gps().ground_speed()*0.8f) {
if (AP_HAL::millis() - _last_consistent_heading > 2000) {
// start using the GPS for heading if the compass has been
// inconsistent with the GPS for 2 seconds
@ -429,6 +429,8 @@ AP_AHRS_DCM::drift_correction_yaw(void)
float yaw_error;
float yaw_deltat;
const AP_GPS &_gps = AP::gps();
if (AP_AHRS_DCM::use_compass()) {
/*
we are using compass for yaw
@ -607,6 +609,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
// we have integrated over
_ra_deltat += deltat;
const AP_GPS &_gps = AP::gps();
if (!have_gps() ||
_gps.status() < AP_GPS::GPS_OK_FIX_3D ||
_gps.num_sats() < _gps_minsats) {
@ -956,6 +960,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0;
location_offset(loc, _position_offset_north, _position_offset_east);
const AP_GPS &_gps = AP::gps();
if (_flags.fly_forward && _have_position) {
float gps_delay_sec = 0;
_gps.get_lag(gps_delay_sec);
@ -983,10 +988,10 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const
ret = true;
}
if (ret && _wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
if (ret && _wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
const float gnd_speed = _gps.ground_speed();
const float gnd_speed = AP::gps().ground_speed();
float true_airspeed = *airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max,

4
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -23,8 +23,8 @@
class AP_AHRS_DCM : public AP_AHRS {
public:
AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps)
: AP_AHRS(ins, baro, gps)
AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro)
: AP_AHRS(ins, baro)
, _omega_I_sum_time(0.0f)
, _renorm_val_sum(0.0f)
, _renorm_val_count(0)

14
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -30,8 +30,12 @@
extern const AP_HAL::HAL& hal;
// constructor
AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags) :
AP_AHRS_DCM(ins, baro, gps),
AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins,
AP_Baro &baro,
NavEKF2 &_EKF2,
NavEKF3 &_EKF3,
Flags flags) :
AP_AHRS_DCM(ins, baro),
EKF2(_EKF2),
EKF3(_EKF3),
_ekf2_started(false),
@ -991,7 +995,7 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
get_filter_status(filt_state);
#endif
}
if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// if the EKF is not fusing GPS and we have a 3D lock, then
// plane and rover would prefer to use the GPS position from
// DCM. This is a safety net while some issues with the EKF
@ -1009,8 +1013,8 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
if (!filt_state.flags.horiz_vel ||
(!filt_state.flags.horiz_pos_abs && !filt_state.flags.horiz_pos_rel)) {
if ((!_compass || !_compass->use_for_yaw()) &&
_gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
_gps.ground_speed() < 2) {
AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D &&
AP::gps().ground_speed() < 2) {
/*
special handling for non-compass mode when sitting
still. The EKF may not yet have aligned its yaw. We

2
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -44,7 +44,7 @@ public:
};
// Constructor
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps,
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro,
NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE);
/* Do not allow copies */

2
libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp
View File

@ -28,7 +28,7 @@ public:
RangeFinder sonar{serial_manager, ROTATION_PITCH_270};
NavEKF2 EKF2{&ahrs, barometer, sonar};
NavEKF3 EKF3{&ahrs, barometer, sonar};
AP_AHRS_NavEKF ahrs{ins, barometer, gps, EKF2, EKF3,
AP_AHRS_NavEKF ahrs{ins, barometer, EKF2, EKF3,
AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
};