From a04412fc1f3f97f04337af7fd67771c54a56dea0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 1 Sep 2020 14:55:39 -0400 Subject: [PATCH] sensors: add SENS_IMU_MODE to optionally skip IMU voting/selection --- src/modules/sensors/sensor_params.c | 12 ++++ src/modules/sensors/sensors.cpp | 14 +++-- .../sensors/vehicle_imu/VehicleIMU.cpp | 20 +++++-- .../sensors/vehicle_imu/VehicleIMU.hpp | 4 +- src/modules/sensors/voted_sensors_update.cpp | 55 ++++++++++++++----- src/modules/sensors/voted_sensors_update.h | 8 ++- 6 files changed, 86 insertions(+), 27 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7afd7bcd9f..53a5b5f58b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -202,3 +202,15 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); * @group Sensors */ PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1); + +/** + * Sensors hub IMU mode + * + * @value 0 Disabled + * @value 1 Publish primary IMU selection + * + * @category system + * @reboot_required true + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_IMU_MODE, 1); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2f71620fd0..5c8cbb75b4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -209,7 +209,8 @@ private: DEFINE_PARAMETERS( (ParamBool) _param_sys_has_baro, - (ParamBool) _param_sys_has_mag + (ParamBool) _param_sys_has_mag, + (ParamBool) _param_sens_imu_mode ) }; @@ -291,11 +292,8 @@ Sensors::~Sensors() bool Sensors::init() { - // initially run manually - ScheduleDelayed(10_ms); - _vehicle_imu_sub[0].registerCallback(); - + ScheduleNow(); return true; } @@ -500,7 +498,11 @@ void Sensors::InitializeVehicleIMU() gyro_sub.copy(&gyro); if (accel.device_id > 0 && gyro.device_id > 0) { - VehicleIMU *imu = new VehicleIMU(i, i); + // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ + // otherwise each VehicleIMU runs in a corresponding INSx WQ + const px4::wq_config_t &wq_config = px4::wq_configurations::nav_and_controllers; + + VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config); if (imu != nullptr) { // Start VehicleIMU instance and store diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 7e91388e84..b0d64ee255 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -45,11 +45,12 @@ using math::constrain; namespace sensors { -VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) : +VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + ScheduledWorkItem(MODULE_NAME, config), _sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index), - _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index) + _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), + _instance(instance) { const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); @@ -82,6 +83,9 @@ VehicleIMU::~VehicleIMU() perf_free(_accel_update_perf); perf_free(_gyro_generation_gap_perf); perf_free(_gyro_update_perf); + + _vehicle_imu_pub.unadvertise(); + _vehicle_imu_status_pub.unadvertise(); } bool VehicleIMU::Start() @@ -89,7 +93,10 @@ bool VehicleIMU::Start() // force initial updates ParametersUpdate(true); - return _sensor_gyro_sub.registerCallback() && _sensor_accel_sub.registerCallback(); + _sensor_gyro_sub.registerCallback(); + _sensor_accel_sub.registerCallback(); + ScheduleNow(); + return true; } void VehicleIMU::Stop() @@ -448,11 +455,12 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) void VehicleIMU::PrintStatus() { if (_accel_calibration.device_id() == _gyro_calibration.device_id()) { - PX4_INFO("IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _accel_calibration.device_id(), + PX4_INFO("%d - IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _instance, _accel_calibration.device_id(), (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval); } else { - PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _accel_calibration.device_id(), + PX4_INFO("%d - Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _instance, + _accel_calibration.device_id(), (double)_accel_interval.update_interval, _gyro_calibration.device_id(), (double)_gyro_interval.update_interval); } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 46f7705364..9d88a94fe2 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -60,7 +60,7 @@ class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem { public: VehicleIMU() = delete; - VehicleIMU(uint8_t accel_index = 0, uint8_t gyro_index = 0); + VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config); ~VehicleIMU() override; @@ -116,6 +116,8 @@ private: bool _intervals_configured{false}; + const uint8_t _instance; + perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")}; perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; perf_counter_t _gyro_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro update interval")}; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index ec45dec825..c292784617 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -137,7 +137,7 @@ void VotedSensorsUpdate::parametersUpdate() void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) { - for (int uorb_index = 0; uorb_index < 3; uorb_index++) { + for (int uorb_index = 0; uorb_index < SENSOR_COUNT_MAX; uorb_index++) { vehicle_imu_s imu_report; if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0) @@ -182,13 +182,40 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) } // find the best sensor - int accel_best_index; - int gyro_best_index; - _accel.voter.get_best(hrt_absolute_time(), &accel_best_index); - _gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index); + int accel_best_index = -1; + int gyro_best_index = -1; - checkFailover(_accel, "Accel"); - checkFailover(_gyro, "Gyro"); + if (_param_sens_imu_mode.get()) { + _accel.voter.get_best(hrt_absolute_time(), &accel_best_index); + _gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index); + + checkFailover(_accel, "Accel"); + checkFailover(_gyro, "Gyro"); + + } else { + + // use sensor_selection to find best + if (_sensor_selection_sub.update(&_selection)) { + // reset inconsistency checks against primary + for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) { + _accel_diff[sensor_index].zero(); + } + + for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) { + _gyro_diff[sensor_index].zero(); + } + } + + for (int i = 0; i < SENSOR_COUNT_MAX; i++) { + if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) { + accel_best_index = i; + } + + if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) { + gyro_best_index = i; + } + } + } // write data for the best sensor to output variables if ((accel_best_index >= 0) && (gyro_best_index >= 0)) { @@ -323,12 +350,14 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) imuPoll(raw); // publish sensor selection if changed - if (_selection_changed) { - // don't publish until selected IDs are valid - if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) { - _selection.timestamp = hrt_absolute_time(); - _sensor_selection_pub.publish(_selection); - _selection_changed = false; + if (_param_sens_imu_mode.get()) { + if (_selection_changed) { + // don't publish until selected IDs are valid + if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) { + _selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(_selection); + _selection_changed = false; + } for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) { _accel_diff[sensor_index].zero(); diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 187396316f..ef068099b1 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -167,9 +167,11 @@ private: uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ uORB::Publication _sensors_status_imu_pub{ORB_ID(sensors_status_imu)}; - uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3]; + uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[SENSOR_COUNT_MAX]; uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; + uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; + sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ @@ -185,6 +187,10 @@ private: uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */ sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ + + DEFINE_PARAMETERS( + (ParamBool) _param_sens_imu_mode + ) }; } /* namespace sensors */