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

AP_InertialSensor: fixed filter setup on 2nd accel/gyro

Browse Source
This commit is contained in:
Andrew Tridgell 2015-11-16 17:31:15 +11:00
parent 0c92331762
commit 32ed0d58f3
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -212,9 +212,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
}
// possibly update filter frequency
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
if (_last_gyro_filter_hz[instance] != _gyro_filter_cutoff()) {
_imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
_last_gyro_filter_hz[instance] = _gyro_filter_cutoff();
}
hal.scheduler->resume_timer_procs();
@ -233,9 +233,9 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance)
}
// possibly update filter frequency
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
if (_last_accel_filter_hz[instance] != _accel_filter_cutoff()) {
_imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
_last_accel_filter_hz[instance] = _accel_filter_cutoff();
}
hal.scheduler->resume_timer_procs();

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -140,8 +140,8 @@ protected:
void update_accel(uint8_t instance);
// support for updating filter at runtime
int8_t _last_accel_filter_hz;
int8_t _last_gyro_filter_hz;
int8_t _last_accel_filter_hz[INS_MAX_INSTANCES];
int8_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor