Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: make get_error_rp() and get_error_yaw() const

Browse Source 
this makes them usable by multiple consumers, and allows use by EKF
This commit is contained in:
Andrew Tridgell 2015-04-21 21:42:17 +10:00
parent 3300de2c9d
commit a0969905ce
2 changed files with 8 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -476,8 +476,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
_omega_I_sum.z += error_z * _ki_yaw * yaw_deltat; _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
} }
_error_yaw_sum += fabsf(yaw_error); _error_yaw = 0.9f * _error_yaw + 0.1f * fabsf(yaw_error);
_error_yaw_count++;
} }
@ -737,8 +736,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
return; return;
} }
_error_rp_sum += best_error; _error_rp = 0.9f * _error_rp + 0.1f * best_error;
_error_rp_count++;
// base the P gain on the spin rate // base the P gain on the spin rate
float spin_rate = _omega.length(); float spin_rate = _omega.length();
@ -873,36 +871,6 @@ AP_AHRS_DCM::euler_angles(void)
update_cd_values(); update_cd_values();
} }
/* reporting of DCM state for MAVLink */
// average error_roll_pitch since last call
float AP_AHRS_DCM::get_error_rp(void)
{
if (_error_rp_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_rp_last;
}
_error_rp_last = _error_rp_sum / _error_rp_count;
_error_rp_sum = 0;
_error_rp_count = 0;
return _error_rp_last;
}
// average error_yaw since last call
float AP_AHRS_DCM::get_error_yaw(void)
{
if (_error_yaw_count == 0) {
// this happens when telemetry is setup on two
// serial ports
return _error_yaw_last;
}
_error_yaw_last = _error_yaw_sum / _error_yaw_count;
_error_yaw_sum = 0;
_error_yaw_count = 0;
return _error_yaw_last;
}
// return our current position estimate using // return our current position estimate using
// dead-reckoning or GPS // dead-reckoning or GPS
bool AP_AHRS_DCM::get_position(struct Location &loc) const bool AP_AHRS_DCM::get_position(struct Location &loc) const

20
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -30,12 +30,8 @@ public:
_omega_I_sum_time(0.0f), _omega_I_sum_time(0.0f),
_renorm_val_sum(0.0f), _renorm_val_sum(0.0f),
_renorm_val_count(0), _renorm_val_count(0),
_error_rp_sum(0.0f), _error_rp(1.0f),
_error_rp_count(0), _error_yaw(1.0f),
_error_rp_last(0.0f),
_error_yaw_sum(0.0f),
_error_yaw_count(0),
_error_yaw_last(0.0f),
_gps_last_update(0), _gps_last_update(0),
_ra_deltat(0.0f), _ra_deltat(0.0f),
_ra_sum_start(0), _ra_sum_start(0),
@ -93,8 +89,8 @@ public:
virtual bool get_position(struct Location &loc) const; virtual bool get_position(struct Location &loc) const;
// status reporting // status reporting
float get_error_rp(void); float get_error_rp(void) const { return _error_rp; }
float get_error_yaw(void); float get_error_yaw(void) const { return _error_yaw; }
// return a wind estimation vector, in m/s // return a wind estimation vector, in m/s
Vector3f wind_estimate(void) { Vector3f wind_estimate(void) {
@ -154,12 +150,8 @@ private:
// state to support status reporting // state to support status reporting
float _renorm_val_sum; float _renorm_val_sum;
uint16_t _renorm_val_count; uint16_t _renorm_val_count;
float _error_rp_sum; float _error_rp;
uint16_t _error_rp_count; float _error_yaw;
float _error_rp_last;
float _error_yaw_sum;
uint16_t _error_yaw_count;
float _error_yaw_last;
// time in millis when we last got a GPS heading // time in millis when we last got a GPS heading
uint32_t _gps_last_update; uint32_t _gps_last_update;