Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: implement notch filter on gyro

Browse Source
This commit is contained in:
Andrew Tridgell 2017-08-16 15:27:58 +10:00
parent 50dcca39bd
commit 7e1368f7a5
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -423,6 +423,12 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, 0), AP_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, 0),
// @Param: NOTCH_
// @DisplayName: Notch filter
// @Description: Gyro notch filter
// @User: Advanced
AP_SUBGROUPINFO(_notch_filter, "NOTCH_", 37, AP_InertialSensor, NotchFilterVector3fParam),
/* /*
NOTE: parameter indexes have gaps above. When adding new NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully parameters check for conflicts carefully
@ -638,6 +644,8 @@ AP_InertialSensor::init(uint16_t sample_rate)
_sample_period_usec = 1000*1000UL / _sample_rate; _sample_period_usec = 1000*1000UL / _sample_rate;
_notch_filter.init(sample_rate);
// establish the baseline time between samples // establish the baseline time between samples
_delta_time = 0; _delta_time = 0;
_next_sample_usec = 0; _next_sample_usec = 0;
@ -1266,6 +1274,9 @@ void AP_InertialSensor::update(void)
} }
} }
// apply notch filter to primary gyro
_gyro[_primary_gyro] = _notch_filter.apply(_gyro[_primary_gyro]);
_last_update_usec = AP_HAL::micros(); _last_update_usec = AP_HAL::micros();
_have_sample = false; _have_sample = false;

4
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -23,6 +23,7 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <Filter/LowPassFilter2p.h> #include <Filter/LowPassFilter2p.h>
#include <Filter/LowPassFilter.h> #include <Filter/LowPassFilter.h>
#include <Filter/NotchFilter.h>
class AP_InertialSensor_Backend; class AP_InertialSensor_Backend;
class AuxiliaryBus; class AuxiliaryBus;
@ -312,6 +313,9 @@ private:
bool _new_accel_data[INS_MAX_INSTANCES]; bool _new_accel_data[INS_MAX_INSTANCES];
bool _new_gyro_data[INS_MAX_INSTANCES]; bool _new_gyro_data[INS_MAX_INSTANCES];
// optional notch filter on gyro
NotchFilterVector3fParam _notch_filter;
// Most recent gyro reading // Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES]; Vector3f _gyro[INS_MAX_INSTANCES];
Vector3f _delta_angle[INS_MAX_INSTANCES]; Vector3f _delta_angle[INS_MAX_INSTANCES];