fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 06:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

move IMU integration out of drivers to sensors hub to handle accel/gyro sync

Browse Source 
- IMU integration move from drivers (PX4Accelerometer/PX4Gyroscope) to sensors/vehicle_imu 
 - sensors: voted_sensors_update now consumes vehicle_imu
 - delete sensor_accel_integrated, sensor_gyro_integrated
 - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status
 - sensors status output minor improvements (ordering, whitespace, show selected sensor device id and instance)
This commit is contained in:
Daniel Agar
2020-05-30 11:07:54 -04:00
committed by GitHub
parent 86cd1d0802
commit e34bdb4be9
74 changed files with 785 additions and 1197 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+27 -48
View File
@@ -68,6 +68,7 @@
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/battery_status.h>
@@ -120,6 +121,12 @@ private:
sensor_combined_s _sensor_combined{};
sensor_preflight_s _sensor_preflight{};
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] {
{this, ORB_ID(vehicle_imu), 0},
{this, ORB_ID(vehicle_imu), 1},
{this, ORB_ID(vehicle_imu), 2}
};
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
@@ -132,12 +139,6 @@ private:
uORB::Publication<sensor_preflight_s> _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */
uORB::Publication<vehicle_magnetometer_s> _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */
uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub[GYRO_COUNT_MAX] {
{this, ORB_ID(sensor_gyro_integrated), 0},
{this, ORB_ID(sensor_gyro_integrated), 1},
{this, ORB_ID(sensor_gyro_integrated), 2}
};
enum class MagCompensationType {
Disabled = 0,
Throttle,
@@ -147,9 +148,6 @@ private:
MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
perf_counter_t _loop_perf; /**< loop performance counter */
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
@@ -211,7 +209,7 @@ Sensors::Sensors(bool hil_enabled) :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
_hil_enabled(hil_enabled),
_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
_voted_sensors_update(_parameters, hil_enabled)
_voted_sensors_update(_parameters, hil_enabled, _vehicle_imu_sub)
{
initialize_parameter_handles(_parameter_handles);
@@ -227,6 +225,11 @@ Sensors::Sensors(bool hil_enabled) :
Sensors::~Sensors()
{
// clear all registered callbacks
for (auto &sub : _vehicle_imu_sub) {
sub.unregisterCallback();
}
_vehicle_acceleration.Stop();
_vehicle_angular_velocity.Stop();
_vehicle_air_data.Stop();
@@ -243,6 +246,8 @@ bool Sensors::init()
// initially run manually
ScheduleDelayed(10_ms);
_vehicle_imu_sub[0].registerCallback();
return true;
}
@@ -417,12 +422,12 @@ void Sensors::InitializeVehicleIMU()
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
if (_vehicle_imu_list[i] == nullptr) {
uORB::Subscription accel_sub{ORB_ID(sensor_accel_integrated), i};
sensor_accel_integrated_s accel{};
uORB::Subscription accel_sub{ORB_ID(sensor_accel), i};
sensor_accel_s accel{};
accel_sub.copy(&accel);
uORB::Subscription gyro_sub{ORB_ID(sensor_gyro_integrated), i};
sensor_gyro_integrated_s gyro{};
uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i};
sensor_gyro_s gyro{};
gyro_sub.copy(&gyro);
if (accel.device_id > 0 && gyro.device_id > 0) {
@@ -432,11 +437,16 @@ void Sensors::InitializeVehicleIMU()
// Start VehicleIMU instance and store
if (imu->Start()) {
_vehicle_imu_list[i] = imu;
ScheduleNow();
} else {
delete imu;
}
}
} else {
// abort on first failure, try again later
return;
}
}
}
@@ -446,7 +456,7 @@ void Sensors::Run()
{
if (should_exit()) {
// clear all registered callbacks
for (auto &sub : _sensor_gyro_integrated_sub) {
for (auto &sub : _vehicle_imu_sub) {
sub.unregisterCallback();
}
@@ -465,9 +475,6 @@ void Sensors::Run()
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
sensor_gyro_integrated_s gyro_integrated;
_sensor_gyro_integrated_sub[_selected_sensor_sub_index].copy(&gyro_integrated);
// check vehicle status for changes to publication state
if (_vcontrol_mode_sub.updated()) {
vehicle_control_mode_s vcontrol_mode{};
@@ -516,32 +523,6 @@ void Sensors::Run()
diff_pres_poll();
// check failover and update subscribed sensor (if necessary)
_voted_sensors_update.checkFailover();
const uint32_t current_device_id = _voted_sensors_update.bestGyroID();
if (_selected_sensor_device_id != current_device_id) {
// clear all registered callbacks
for (auto &sub : _sensor_gyro_integrated_sub) {
sub.unregisterCallback();
}
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
sensor_gyro_integrated_s report{};
if (_sensor_gyro_integrated_sub[i].copy(&report)) {
if ((report.device_id != 0) && (report.device_id == current_device_id)) {
if (_sensor_gyro_integrated_sub[i].registerCallback()) {
// record selected sensor (array index)
_selected_sensor_sub_index = i;
_selected_sensor_device_id = current_device_id;
}
}
}
}
}
if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
_magnetometer_pub.publish(magnetometer);
_magnetometer_prev_timestamp = magnetometer.timestamp;
@@ -639,7 +620,9 @@ int Sensors::print_status()
_voted_sensors_update.printStatus();
PX4_INFO_RAW("\n");
_vehicle_air_data.PrintStatus();
PX4_INFO_RAW("\n");
PX4_INFO("Airspeed status:");
_airspeed_validator.print();
@@ -649,14 +632,10 @@ int Sensors::print_status()
PX4_INFO_RAW("\n");
_vehicle_angular_velocity.PrintStatus();
PX4_INFO_RAW("\n");
_vehicle_air_data.PrintStatus();
PX4_INFO_RAW("\n");
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
PX4_INFO_RAW("\n");
i->PrintStatus();
}
}