5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00

AP_AHRS: removed the AHRS_GPS_DELAY parameter

the best value has turned out to be 1, and tweaking it has not turned
out to be useful, so this simplifies the code in preparation for
adding the anti-aliasing handling with multiple accelerometers
This commit is contained in:
Andrew Tridgell 2014-02-27 08:57:44 +11:00
parent 6b9e1edf38
commit 0b45d2bc06
3 changed files with 5 additions and 51 deletions

View File

@ -108,13 +108,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
// @Param: GPS_DELAY
// @DisplayName: AHRS GPS delay steps
// @Description: number of GPS samples to delay accels for synchronisation with the GPS velocity data
// @Range: 0 5
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("GPS_DELAY", 12, AP_AHRS, _gps_delay, 1),
// NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay
// of 1 was found to be the best choice
#if AP_AHRS_NAVEKF_AVAILABLE
// @Param: EKF_USE

View File

@ -461,48 +461,9 @@ AP_AHRS_DCM::drift_correction_yaw(void)
*/
Vector3f AP_AHRS_DCM::ra_delayed(const Vector3f &ra)
{
if (_ra_delay_length != _gps_delay.get()) {
// the AHRS_GPS_DELAY setting has changed
// constrain it between 0 and 5
if (_gps_delay.get() > 5) {
_gps_delay.set(5);
}
if (_gps_delay.get() < 0) {
_gps_delay.set(0);
}
if (_ra_delay_buffer != NULL) {
delete[] _ra_delay_buffer;
_ra_delay_buffer = NULL;
}
// allocate the new buffer
_ra_delay_length = _gps_delay.get();
if (_ra_delay_length != 0) {
_ra_delay_buffer = new Vector3f[_ra_delay_length];
}
_ra_delay_next = 0;
if (_ra_delay_buffer != NULL) {
// on size change prefill the buffer with the current value
for (uint8_t i=0; i<_ra_delay_length; i++) {
_ra_delay_buffer[i] = ra;
}
}
}
if (_ra_delay_buffer == NULL) {
// we're not doing any delay
return ra;
}
// get the old element, and then fill it with the new element
Vector3f ret = _ra_delay_buffer[_ra_delay_next];
_ra_delay_buffer[_ra_delay_next] = ra;
// move to the next element
_ra_delay_next++;
if (_ra_delay_next == _ra_delay_length) {
_ra_delay_next = 0;
}
Vector3f ret = _ra_delay_buffer;
_ra_delay_buffer = ra;
return ret;
}

View File

@ -111,9 +111,7 @@ private:
// variables to cope with delaying the GA sum to match GPS lag
Vector3f ra_delayed(const Vector3f &ra);
uint8_t _ra_delay_length;
uint8_t _ra_delay_next;
Vector3f *_ra_delay_buffer;
Vector3f _ra_delay_buffer;
// P term gain based on spin rate
float _P_gain(float spin_rate);