Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

uORB: add Subscription method to change instance #15656

Merged
merged 4 commits into from Sep 3, 2020
Merged

uORB: add Subscription method to change instance #15656

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
277620f
uORB: add Subscription method to change instance
dagar Sep 1, 2020
b6293c2
sensors/vehicle_angular_velocity: use single uORB::Subscription, but …
dagar Sep 1, 2020
adef32f
sensors/vehicle_acceleration: use single uORB::Subscription, but chan…
dagar Sep 1, 2020
37d906d
ekf2: use single uORB::Subscription, but change instance
dagar Sep 1, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
19 changes: 6 additions & 13 deletions src/modules/ekf2/ekf2_main.cpp
View file
Open in desktop
Expand Up @@ -247,11 +247,7 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams, public px4::Sch

uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
static constexpr int MAX_SENSOR_COUNT = 3;
uORB::SubscriptionCallbackWorkItem _vehicle_imu_subs[MAX_SENSOR_COUNT] {
{this, ORB_ID(vehicle_imu), 0},
{this, ORB_ID(vehicle_imu), 1},
{this, ORB_ID(vehicle_imu), 2}
};
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
int _imu_sub_index{-1};
bool _callback_registered{false};
int _lockstep_component{-1};
Expand Down Expand Up @@ -670,11 +666,11 @@ bool Ekf2::init()
// if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined)
if (device_id != 0) {
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
vehicle_imu_s imu{};
vehicle_imu_s imu;

if (_vehicle_imu_subs[i].copy(&imu)) {
if (_vehicle_imu_sub.ChangeInstance(i) && _vehicle_imu_sub.copy(&imu)) {
if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) {
if (_vehicle_imu_subs[i].registerCallback()) {
if (_vehicle_imu_sub.registerCallback()) {
PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id);
_imu_sub_index = i;
_callback_registered = true;
Expand Down Expand Up @@ -749,10 +745,7 @@ void Ekf2::Run()
{
if (should_exit()) {
_sensor_combined_sub.unregisterCallback();

for (auto &i : _vehicle_imu_subs) {
i.unregisterCallback();
}
_vehicle_imu_sub.unregisterCallback();

exit_and_cleanup();
return;
Expand All @@ -771,7 +764,7 @@ void Ekf2::Run()

if (_imu_sub_index >= 0) {
vehicle_imu_s imu;
updated = _vehicle_imu_subs[_imu_sub_index].update(&imu);
updated = _vehicle_imu_sub.update(&imu);

imu_sample_new.time_us = imu.timestamp_sample;
imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;
Expand Down
90 changes: 36 additions & 54 deletions src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View file
Open in desktop
Expand Up @@ -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();
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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)
Expand All @@ -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;
Expand Down Expand Up @@ -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;

if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
while (_sensor_sub.update(&sensor_data)) {

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++;
// 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;
}

_timestamp_sample_last = sensor_data.timestamp_sample;
}
} else {
_interval_sum = 0.f;
_interval_count = 0.f;
}

CheckFilters();
_timestamp_sample_last = sensor_data.timestamp_sample;

// 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);
CheckFilters();

_acceleration_prev = 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);

// publish once all new samples are processed
sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
_acceleration_prev = accel_corrected;

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);
// 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);

return;
}
return;
}
}
}
Expand Down
6 changes: 1 addition & 5 deletions src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View file
Open in desktop
Expand Up @@ -81,11 +81,7 @@ class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem
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{};

Expand Down