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

Multi-EKF support (ekf2)

Browse Source 
- ekf2 can now run in multi-instance mode (currently up to 9 instances)
    - in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
 - new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
 - sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
 - existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
 - ekf2 single instance mode is still fully supported and the default

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
This commit is contained in:
Daniel Agar
2020-10-27 10:56:11 -04:00
committed by GitHub
co-authored by Paul Riseborough
parent d5245a22d3
commit 0f411d6820
56 changed files with 1747 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+10 -4
View File
@@ -199,7 +199,7 @@ void VehicleMagnetometer::Run()
if (!_advertised[uorb_index]) {
// use data's timestamp to throttle advertisement checks
if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) {
if (_sensor_sub[uorb_index].advertised()) {
if (uorb_index > 0) {
/* the first always exists, but for each further sensor, add a new validator */
@@ -217,12 +217,18 @@ void VehicleMagnetometer::Run()
}
}
if (_selected_sensor_sub_index < 0) {
_sensor_sub[uorb_index].registerCallback();
}
} else {
_last_data[uorb_index].timestamp = hrt_absolute_time();
}
}
} else {
}
if (_advertised[uorb_index]) {
sensor_mag_s report;
while (_sensor_sub[uorb_index].update(&report)) {
@@ -339,8 +345,8 @@ void VehicleMagnetometer::Run()
void VehicleMagnetometer::Publish(uint8_t instance, bool multi)
{
if ((_param_sens_mag_rate.get() > 0)
&& hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())) {
if ((_param_sens_mag_rate.get() > 0) && (_last_publication_timestamp[instance] ||
(hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())))) {
const Vector3f magnetometer_data = _mag_sum[instance] / _mag_sum_count[instance];
const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _mag_sum_count[instance];