fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 20:57:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_acceleration: use single uORB::Subscription, but change instance

Browse Source
This commit is contained in:
Daniel Agar
2020-09-01 13:54:26 -04:00
parent ee88561a33
commit 59e66e5be9
2 changed files with 37 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+36 -54
View File
@@ -74,10 +74,7 @@ bool VehicleAcceleration::Start()
void VehicleAcceleration::Stop()
{
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
_sensor_sub.unregisterCallback();
_sensor_selection_sub.unregisterCallback();
Deinit();
@@ -107,11 +104,11 @@ void VehicleAcceleration::CheckFilters()
const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
(float)sensor_accel_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;
}
}
@@ -161,17 +158,12 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
_sensor_selection_sub.copy(&sensor_selection);
if (_selected_sensor_device_id != sensor_selection.accel_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_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_accel_s report{};
_sensor_sub[i].copy(&report);
if ((sensor_accel_sub.get().device_id != 0) && (sensor_accel_sub.get().device_id == sensor_selection.accel_device_id)) {
if ((report.device_id != 0) && (report.device_id == sensor_selection.accel_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)
@@ -181,7 +173,7 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_calibration.set_device_id(report.device_id);
_calibration.set_device_id(sensor_accel_sub.get().device_id);
// reset sample interval accumulator on sensor change
_timestamp_sample_last = 0;
@@ -227,54 +219,44 @@ void VehicleAcceleration::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_accel_s sensor_data;
sensor_accel_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;
_timestamp_sample_last = sensor_data.timestamp_sample;
}
CheckFilters();
CheckFilters();
// Apply calibration and filter
// - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
// - biquad low-pass filter
const Vector3f accel_corrected = _calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias;
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
// Apply calibration and filter
// - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
// - biquad low-pass filter
const Vector3f accel_corrected = _calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias;
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
_acceleration_prev = accel_corrected;
_acceleration_prev = accel_corrected;
// publish once all new samples are processed
if (!_sensor_sub.updated()) {
// Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel_filtered.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration);
// publish once all new samples are processed
sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
if (!sensor_updated) {
// Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel_filtered.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration);
return;
}
return;
}
}
}
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+1 -5
View File
@@ -81,11 +81,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_accel), 0},
{this, ORB_ID(sensor_accel), 1},
{this, ORB_ID(sensor_accel), 2}
};
uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)};
calibration::Accelerometer _calibration{};