zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
Daniel Agar 3 years ago
parent
3a37fd92e6
commit
f9d87fd97a
1 changed files with 20 additions and 21 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

29
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
Unescape 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++) { 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}; 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 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; 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) if (sensor_gyro_fifo_sub.get().device_id == device_id) {
&& _sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) { if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) {
// make sure non-FIFO sub is unregistered // make sure non-FIFO sub is unregistered
_sensor_sub.unregisterCallback(); _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; _selected_sensor_device_id = sensor_gyro_fifo_sub.get().device_id;
_timestamp_sample_last = 0; _timestamp_sample_last = 0;
@ -288,7 +287,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
return true; return true;
} else { } 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++) { for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), 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 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; 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) if (sensor_gyro_sub.get().device_id == device_id) {
&& _sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
// make sure FIFO sub is unregistered // make sure FIFO sub is unregistered
_sensor_fifo_sub.unregisterCallback(); _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; _selected_sensor_device_id = sensor_gyro_sub.get().device_id;
_timestamp_sample_last = 0; _timestamp_sample_last = 0;
@ -325,7 +325,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
return true; return true;
} else { } 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) { if (device_id != 0) {
PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", device_id); 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; _selected_sensor_device_id = 0;

Write Preview
Loading…
Cancel
Save