yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

sensors: add SENS_IMU_MODE to optionally skip IMU voting/selection

Browse Source
This commit is contained in:
Daniel Agar
2020-09-01 14:55:39 -04:00
parent 5ebc4d31da
commit a04412fc1f
6 changed files with 86 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/sensors/sensor_params.c
View File

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

14
src/modules/sensors/sensors.cpp
View File

@@ -209,7 +209,8 @@ private:
DEFINE_PARAMETERS(
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
(ParamBool<px4::params::SENS_IMU_MODE>) _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

20
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@@ -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);
}

4
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@@ -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")};

55
src/modules/sensors/voted_sensors_update.cpp
View File

@@ -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();

8
src/modules/sensors/voted_sensors_update.h
View File

@@ -167,9 +167,11 @@ private:
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::Publication<sensors_status_imu_s> _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_s, ACCEL_COUNT_MAX> _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<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
)
};
} /* namespace sensors */