fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 05:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Q estimator: Use delta angles when available

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-05 17:27:43 +02:00
parent 536321f5dd
commit 1f66c26a62
1 changed files with 23 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+23 -2
View File
@@ -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 ||