aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: use compass reference rather than pointer

Browse Source
This commit is contained in:
Peter Barker 2021-07-30 14:41:09 +10:00 committed by Andrew Tridgell
parent f3a52999d8
commit d131cf01d3
2 changed files with 14 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -346,9 +346,9 @@ AP_AHRS_DCM::normalize(void)
// produce a yaw error value. The returned value is proportional
// to sin() of the current heading error in earth frame
float
AP_AHRS_DCM::yaw_error_compass(Compass *_compass)
AP_AHRS_DCM::yaw_error_compass(Compass &compass)
{
const Vector3f &mag = _compass->get_field();
const Vector3f &mag = compass.get_field();
// get the mag vector in the earth frame
Vector2f rb = _dcm_matrix.mulXY(mag);
@ -363,8 +363,8 @@ AP_AHRS_DCM::yaw_error_compass(Compass *_compass)
}
// update vector holding earths magnetic field (if required)
if( !is_equal(_last_declination,_compass->get_declination()) ) {
_last_declination = _compass->get_declination();
if( !is_equal(_last_declination, compass.get_declination()) ) {
_last_declination = compass.get_declination();
_mag_earth.x = cosf(_last_declination);
_mag_earth.y = sinf(_last_declination);
}
@ -434,10 +434,9 @@ bool AP_AHRS_DCM::use_fast_gains(void) const
// return true if we should use the compass for yaw correction
bool AP_AHRS_DCM::use_compass(void)
{
Compass &compass = AP::compass();
Compass *_compass = &compass;
const Compass &compass = AP::compass();
if (!_compass || !_compass->use_for_yaw()) {
if (!compass.use_for_yaw()) {
// no compass available
return false;
}
@ -489,9 +488,8 @@ AP_AHRS_DCM::drift_correction_yaw(void)
const AP_GPS &_gps = AP::gps();
Compass &compass = AP::compass();
Compass *_compass = &compass;
if (_compass && _compass->is_calibrating()) {
if (compass.is_calibrating()) {
// don't do any yaw correction while calibrating
return;
}
@ -500,20 +498,20 @@ AP_AHRS_DCM::drift_correction_yaw(void)
/*
we are using compass for yaw
*/
if (_compass->last_update_usec() != _compass_last_update) {
yaw_deltat = (_compass->last_update_usec() - _compass_last_update) * 1.0e-6f;
_compass_last_update = _compass->last_update_usec();
if (compass.last_update_usec() != _compass_last_update) {
yaw_deltat = (compass.last_update_usec() - _compass_last_update) * 1.0e-6f;
_compass_last_update = compass.last_update_usec();
// we force an additional compass read()
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_flags.have_initial_yaw && _compass->read()) {
const float heading = _compass->calculate_heading(_dcm_matrix);
if (!_flags.have_initial_yaw && compass.read()) {
const float heading = compass.calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass(_compass);
yaw_error = yaw_error_compass(compass);
// also update the _gps_last_update, so if we later
// disable the compass due to significant yaw error we

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -128,7 +128,7 @@ private:
bool renorm(Vector3f const &a, Vector3f &result);
void drift_correction(float deltat);
void drift_correction_yaw(void);
float yaw_error_compass(Compass *_compass);
float yaw_error_compass(class Compass &compass);
void euler_angles(void);
bool have_gps(void) const;
bool use_fast_gains(void) const;