dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

attitude_estimator_q: filter accel and gyro data 

Since accel and gyro are not filtered in the drivers anymore, we need to
filter them in this estimator in order to achieve a similar performance.
This commit is contained in:
Julian Oes 2016-08-03 17:23:22 +02:00 committed by Lorenz Meier
parent cfa203ca22
commit d748f6ca71
1 changed files with 23 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -178,6 +178,14 @@ private:
math::LowPassFilter2p _lp_pitch_rate;
math::LowPassFilter2p _lp_yaw_rate;
/* Low pass filter for accel/gyro */
math::LowPassFilter2p _lp_accel_x;
math::LowPassFilter2p _lp_accel_y;
math::LowPassFilter2p _lp_accel_z;
math::LowPassFilter2p _lp_gyro_x;
math::LowPassFilter2p _lp_gyro_y;
math::LowPassFilter2p _lp_gyro_z;
hrt_abstime _vel_prev_t = 0;
bool _inited = false;
@ -207,7 +215,13 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_pos_acc(0, 0, 0),
_lp_roll_rate(250.0f, 30.0f),
_lp_pitch_rate(250.0f, 30.0f),
_lp_yaw_rate(250.0f, 20.0f)
_lp_yaw_rate(250.0f, 20.0f),
_lp_accel_x(250.0f, 30.0f),
_lp_accel_y(250.0f, 30.0f),
_lp_accel_z(250.0f, 30.0f),
_lp_gyro_x(250.0f, 30.0f),
_lp_gyro_y(250.0f, 30.0f),
_lp_gyro_z(250.0f, 30.0f)
{
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
@ -328,15 +342,17 @@ void AttitudeEstimatorQ::task_main()
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
// Filter gyro signal since it is not fildered in the drivers.
_gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]);
_gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]);
_gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]);
}
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_accel(0) = sensors.accelerometer_m_s2[0];
_accel(1) = sensors.accelerometer_m_s2[1];
_accel(2) = sensors.accelerometer_m_s2[2];
// Filter accel signal since it is not fildered in the drivers.
_accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]);
_accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]);
_accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);
if (_accel.length() < 0.01f) {
warnx("WARNING: degenerate accel!");