fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 11:13:34 +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/voted_sensors_update.cpp
+39 -16
View File
@@ -374,14 +374,19 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
status.accel_device_id_primary = _selection.accel_device_id;
status.gyro_device_id_primary = _selection.gyro_device_id;
static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof(
sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof(
sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (_accel_device_id[i] != 0) {
if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) {
status.accel_device_ids[i] = _accel_device_id[i];
status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm();
status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
}
if (_gyro_device_id[i] != 0) {
if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) {
status.gyro_device_ids[i] = _gyro_device_id[i];
status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm();
status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
@@ -403,32 +408,50 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
void VotedSensorsUpdate::calcAccelInconsistency()
{
const Vector3f primary_accel{_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2};
Vector3f accel_mean{};
Vector3f accel_all[MAX_SENSOR_COUNT] {};
uint8_t accel_count = 0;
// Check each sensor against the primary
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
// check that the sensor we are checking against is not the same as the primary
if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0)
&& (_accel_device_id[sensor_index] != _selection.accel_device_id)) {
if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
accel_count++;
accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2};
accel_mean += accel_all[sensor_index];
}
}
const Vector3f current_accel{_last_sensor_data[sensor_index].accelerometer_m_s2};
_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (primary_accel - current_accel);
if (accel_count > 0) {
accel_mean /= accel_count;
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean);
}
}
}
}
void VotedSensorsUpdate::calcGyroInconsistency()
{
const Vector3f primary_gyro{_last_sensor_data[_gyro.last_best_vote].gyro_rad};
Vector3f gyro_mean{};
Vector3f gyro_all[MAX_SENSOR_COUNT] {};
uint8_t gyro_count = 0;
// Check each sensor against the primary
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
// check that the sensor we are checking against is not the same as the primary
if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0)
&& (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) {
if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
gyro_count++;
gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad};
gyro_mean += gyro_all[sensor_index];
}
}
const Vector3f current_gyro{_last_sensor_data[sensor_index].gyro_rad};
_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (primary_gyro - current_gyro);
if (gyro_count > 0) {
gyro_mean /= gyro_count;
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean);
}
}
}
}