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

sensors/vehicle_angular_velocity: improve error handling (especially during initial selection)

Browse Source
This commit is contained in:
Daniel Agar 2022-02-07 11:35:36 -05:00
parent 3a37fd92e6
commit f9d87fd97a
1 changed files with 20 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -259,21 +259,20 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
uORB::SubscriptionData<sensor_gyro_fifo_s> sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i};
if (sensor_gyro_fifo_sub.get().device_id != 0) {
if ((time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s) && (sensor_gyro_fifo_sub.get().device_id != 0)) {
// if no gyro was selected use the first valid sensor_gyro_fifo
if (!device_id_valid && (time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s)) {
if (!device_id_valid) {
device_id = sensor_gyro_fifo_sub.get().device_id;
PX4_WARN("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id);
}
if ((sensor_gyro_fifo_sub.get().device_id == device_id)
&& _sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) {
if (sensor_gyro_fifo_sub.get().device_id == device_id) {
if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) {
// make sure non-FIFO sub is unregistered
_sensor_sub.unregisterCallback();
// make sure non-FIFO sub is unregistered
_sensor_sub.unregisterCallback();
_calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id);
_calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id);
if (_calibration.enabled()) {
_selected_sensor_device_id = sensor_gyro_fifo_sub.get().device_id;
_timestamp_sample_last = 0;
@ -288,7 +287,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
return true;
} else {
_selected_sensor_device_id = 0;
PX4_ERR("unable to register callback for sensor_gyro_fifo:%" PRIu8 " %" PRIu32,
i, sensor_gyro_fifo_sub.get().device_id);
}
}
}
@ -297,20 +297,20 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
if (sensor_gyro_sub.get().device_id != 0) {
if ((time_now_us < sensor_gyro_sub.get().timestamp + 1_s) && (sensor_gyro_sub.get().device_id != 0)) {
// if no gyro was selected use the first valid sensor_gyro
if (!device_id_valid && (time_now_us < sensor_gyro_sub.get().timestamp + 1_s)) {
if (!device_id_valid) {
device_id = sensor_gyro_sub.get().device_id;
PX4_WARN("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
}
if ((sensor_gyro_sub.get().device_id == device_id)
&& _sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
// make sure FIFO sub is unregistered
_sensor_fifo_sub.unregisterCallback();
if (sensor_gyro_sub.get().device_id == device_id) {
if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
// make sure FIFO sub is unregistered
_sensor_fifo_sub.unregisterCallback();
_calibration.set_device_id(sensor_gyro_sub.get().device_id);
_calibration.set_device_id(sensor_gyro_sub.get().device_id);
if (_calibration.enabled()) {
_selected_sensor_device_id = sensor_gyro_sub.get().device_id;
_timestamp_sample_last = 0;
@ -325,7 +325,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
return true;
} else {
_selected_sensor_device_id = 0;
PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32,
i, sensor_gyro_sub.get().device_id);
}
}
}
@ -333,8 +334,6 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
if (device_id != 0) {
PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", device_id);
print_message(ORB_ID(sensor_selection), sensor_selection);
}
_selected_sensor_device_id = 0;