From 1f66c26a6256b5ae9c81b39fac0f86b3260f5ac0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Sep 2015 17:27:43 +0200 Subject: [PATCH] Q estimator: Use delta angles when available --- .../attitude_estimator_q_main.cpp | 25 +++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 7e1dd26ca5..9dd2a38071 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -227,7 +227,7 @@ int AttitudeEstimatorQ::start() _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 2100, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); @@ -292,7 +292,23 @@ void AttitudeEstimatorQ::task_main() // Feed validator with recent sensor data for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) { - _voter_gyro.put(i, sensors.gyro_timestamp[i], &sensors.gyro_rad_s[i * 3], sensors.gyro_errcount[i]); + + /* ignore empty fields */ + if (sensors.gyro_timestamp[i] > 0) { + + float gyro[3]; + + for (unsigned j = 0; j < 3; j++) { + if (sensors.gyro_integral_dt[i] > 0) { + gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); + } else { + /* fall back to angular rate */ + gyro[j] = sensors.gyro_rad_s[i * 3 + j]; + } + } + + _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i]); + } _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], sensors.accelerometer_errcount[i]); _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i]); } @@ -305,6 +321,11 @@ void AttitudeEstimatorQ::task_main() _accel.set(_voter_accel.get_best(curr_time, &best_accel)); _mag.set(_voter_mag.get_best(curr_time, &best_mag)); + if (_accel.length() < 0.01f || _mag.length() < 0.01f) { + warnx("WARNING: degenerate accel / mag!"); + continue; + } + _data_good = true; if (!_failsafe && (_voter_gyro.failover_count() > 0 ||