diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index cdf837217d..6c5cd40973 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -77,10 +77,7 @@ bool VehicleAngularVelocity::Start() void VehicleAngularVelocity::Stop() { // clear all registered callbacks - for (auto &sub : _sensor_sub) { - sub.unregisterCallback(); - } - + _sensor_sub.unregisterCallback(); _sensor_selection_sub.unregisterCallback(); Deinit(); @@ -110,11 +107,11 @@ void VehicleAngularVelocity::CheckFilters() const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f, (float)sensor_gyro_s::ORB_QUEUE_LENGTH); - _sensor_sub[_selected_sensor_sub_index].set_required_updates(samples); + _sensor_sub.set_required_updates(samples); _required_sample_updates = samples; } else { - _sensor_sub[_selected_sensor_sub_index].set_required_updates(1); + _sensor_sub.set_required_updates(1); _required_sample_updates = 1; } } @@ -181,17 +178,12 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) _sensor_selection_sub.copy(&sensor_selection); if (_selected_sensor_device_id != sensor_selection.gyro_device_id) { - // clear all registered callbacks - for (auto &sub : _sensor_sub) { - sub.unregisterCallback(); - } + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - sensor_gyro_s report{}; - _sensor_sub[i].copy(&report); + if ((sensor_gyro_sub.get().device_id != 0) && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) { - if ((report.device_id != 0) && (report.device_id == sensor_selection.gyro_device_id)) { - if (_sensor_sub[i].registerCallback()) { + if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i); // record selected sensor (array index) @@ -201,7 +193,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) // clear bias and corrections _bias.zero(); - _calibration.set_device_id(report.device_id); + _calibration.set_device_id(sensor_gyro_sub.get().device_id); // reset sample interval accumulator on sensor change _timestamp_sample_last = 0; @@ -247,88 +239,78 @@ void VehicleAngularVelocity::Run() SensorBiasUpdate(selection_updated); ParametersUpdate(); - bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); - // process all outstanding messages - while (sensor_updated || selection_updated) { - selection_updated = false; + sensor_gyro_s sensor_data; - sensor_gyro_s sensor_data; + while (_sensor_sub.update(&sensor_data)) { - if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) { + // collect sample interval average for filters + if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) { + _interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last); + _interval_count++; - if (sensor_updated) { - // collect sample interval average for filters - if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) { - _interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last); - _interval_count++; + } else { + _interval_sum = 0.f; + _interval_count = 0.f; + } - } else { - _interval_sum = 0.f; - _interval_count = 0.f; + _timestamp_sample_last = sensor_data.timestamp_sample; + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f); + _timestamp_sample_prev = sensor_data.timestamp_sample; + + // get the sensor data and correct for thermal errors (apply offsets and scale) + const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + + // correct for in-run bias errors + const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias; + + // Gyro filtering: + // - Apply general notch filter (IMU_GYRO_NF_FREQ) + // - Apply general low-pass filter (IMU_GYRO_CUTOFF) + // - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + + const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)}; + + const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)}; + + const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt; + _angular_velocity_prev = angular_velocity; + _angular_acceleration_prev = angular_acceleration_raw; + const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)}; + + CheckFilters(); + + // publish once all new samples are processed + if (!_sensor_sub.updated()) { + bool publish = true; + + if (_param_imu_gyro_rate_max.get() > 0) { + const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); + + if (hrt_elapsed_time(&_last_publish) < interval) { + publish = false; } - - _timestamp_sample_last = sensor_data.timestamp_sample; } - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f); - _timestamp_sample_prev = sensor_data.timestamp_sample; + if (publish) { + // Publish vehicle_angular_acceleration + vehicle_angular_acceleration_s v_angular_acceleration; + v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample; + angular_acceleration.copyTo(v_angular_acceleration.xyz); + v_angular_acceleration.timestamp = hrt_absolute_time(); + _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); - // get the sensor data and correct for thermal errors (apply offsets and scale) - const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + // Publish vehicle_angular_velocity + vehicle_angular_velocity_s v_angular_velocity; + v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample; + angular_velocity.copyTo(v_angular_velocity.xyz); + v_angular_velocity.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_pub.publish(v_angular_velocity); - // correct for in-run bias errors - const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias; - - // Gyro filtering: - // - Apply general notch filter (IMU_GYRO_NF_FREQ) - // - Apply general low-pass filter (IMU_GYRO_CUTOFF) - // - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) - - const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)}; - - const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)}; - - const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt; - _angular_velocity_prev = angular_velocity; - _angular_acceleration_prev = angular_acceleration_raw; - const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)}; - - CheckFilters(); - - // publish once all new samples are processed - sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); - - if (!sensor_updated) { - bool publish = true; - - if (_param_imu_gyro_rate_max.get() > 0) { - const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); - - if (hrt_elapsed_time(&_last_publish) < interval) { - publish = false; - } - } - - if (publish) { - // Publish vehicle_angular_acceleration - vehicle_angular_acceleration_s v_angular_acceleration; - v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample; - angular_acceleration.copyTo(v_angular_acceleration.xyz); - v_angular_acceleration.timestamp = hrt_absolute_time(); - _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); - - // Publish vehicle_angular_velocity - vehicle_angular_velocity_s v_angular_velocity; - v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample; - angular_velocity.copyTo(v_angular_velocity.xyz); - v_angular_velocity.timestamp = hrt_absolute_time(); - _vehicle_angular_velocity_pub.publish(v_angular_velocity); - - _last_publish = v_angular_velocity.timestamp_sample; - return; - } + _last_publish = v_angular_velocity.timestamp_sample; + return; } } } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 3596ae773f..7d9ab944ea 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -84,11 +84,7 @@ private: uORB::Subscription _params_sub{ORB_ID(parameter_update)}; uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; - uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { - {this, ORB_ID(sensor_gyro), 0}, - {this, ORB_ID(sensor_gyro), 1}, - {this, ORB_ID(sensor_gyro), 2} - }; + uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)}; calibration::Gyroscope _calibration{};