Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:37:27 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

sensors: publish sensor_selection initially regardless of SENS_IMU_MODE

Browse Source 
- in multi-EKF mode the EKF selector becomes repsonsible for sensor
selector rather than the sensors module
 - this updates the sensors module to still make the initial primary IMU
selection on startup before the EKF selector (including if the
estimators never fully initialize)
This commit is contained in:
Daniel Agar
2021-02-26 21:16:07 -05:00
committed by Lorenz Meier
parent 9d47f7ecda
commit 9d0c966b15
1 changed files with 26 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+26 -26
View File
@@ -186,15 +186,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
int accel_best_index = -1;
int gyro_best_index = -1;
if (_param_sens_imu_mode.get()) {
_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
checkFailover(_accel, "Accel");
checkFailover(_gyro, "Gyro");
} else {
if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
// use sensor_selection to find best
if (_sensor_selection_sub.update(&_selection)) {
// reset inconsistency checks against primary
@@ -216,6 +208,14 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
gyro_best_index = i;
}
}
} else {
// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
checkFailover(_accel, "Accel");
checkFailover(_gyro, "Gyro");
}
// write data for the best sensor to output variables
@@ -255,6 +255,23 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
}
}
}
// publish sensor selection if changed
if (_param_sens_imu_mode.get() || (_selection.timestamp == 0)) {
if (_selection_changed) {
// don't publish until selected IDs are valid
if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) {
_selection.timestamp = hrt_absolute_time();
_sensor_selection_pub.publish(_selection);
_selection_changed = false;
}
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
_gyro_diff[sensor_index].zero();
}
}
}
}
bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name)
@@ -350,23 +367,6 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
{
imuPoll(raw);
// publish sensor selection if changed
if (_param_sens_imu_mode.get()) {
if (_selection_changed) {
// don't publish until selected IDs are valid
if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) {
_selection.timestamp = hrt_absolute_time();
_sensor_selection_pub.publish(_selection);
_selection_changed = false;
}
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
_gyro_diff[sensor_index].zero();
}
}
}
calcAccelInconsistency();
calcGyroInconsistency();