mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-16 21:27:36 +08:00
sensors/vehicle_angular_velocity: use single uORB::Subscription, but change instance
This commit is contained in:
Daniel Agar
@@ -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_s> 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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{};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user