dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: don't store sensor_selection_s copy

This commit is contained in:
Daniel Agar 2020-10-27 14:14:00 -04:00
parent db5235b074
commit 7757aeda94
2 changed files with 10 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/ekf2/EKF2.cpp
View File

@ -355,23 +355,19 @@ void EKF2::Run()
}
// Always update sensor selction first time through if time stamp is non zero
if (!_multi_mode && (_sensor_selection_sub.updated() || (_sensor_selection.timestamp == 0))) {
const sensor_selection_s sensor_selection_prev = _sensor_selection;
if (!_multi_mode && (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0))) {
sensor_selection_s sensor_selection;
if (_sensor_selection_sub.copy(&_sensor_selection)) {
if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
_imu_bias_reset_request = true;
}
if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
_imu_bias_reset_request = true;
}
if (_sensor_selection_sub.copy(&sensor_selection)) {
if (_device_id_accel != sensor_selection.accel_device_id) {
_imu_bias_reset_request = true;
_device_id_accel = sensor_selection.accel_device_id;
}
_device_id_accel = _sensor_selection.accel_device_id;
_device_id_gyro = _sensor_selection.gyro_device_id;
if (_device_id_gyro != sensor_selection.gyro_device_id) {
_imu_bias_reset_request = true;
_device_id_gyro = sensor_selection.gyro_device_id;
}
}
}

1
src/modules/ekf2/EKF2.hpp
View File

@ -213,7 +213,6 @@ private:
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
sensor_selection_s _sensor_selection{};
vehicle_land_detected_s _vehicle_land_detected{};
vehicle_status_s _vehicle_status{};