fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 11:40: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/ekf2/ekf2_main.cpp
+3 -5
View File
@@ -772,9 +772,9 @@ void Ekf2::Run()
updated = _vehicle_imu_subs[_imu_sub_index].update(&imu);
imu_sample_new.time_us = imu.timestamp_sample;
imu_sample_new.delta_ang_dt = imu.dt * 1.e-6f;
imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;
imu_sample_new.delta_ang = Vector3f{imu.delta_angle};
imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f;
imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
if (imu.delta_velocity_clipping > 0) {
@@ -783,7 +783,7 @@ void Ekf2::Run()
imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
}
imu_dt = imu.dt;
imu_dt = imu.delta_angle_dt;
bias.accel_device_id = imu.accel_device_id;
bias.gyro_device_id = imu.gyro_device_id;
@@ -853,12 +853,10 @@ void Ekf2::Run()
if (_imu_sub_index < 0) {
if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
PX4_WARN("accel id changed, resetting IMU bias");
_imu_bias_reset_request = true;
}
if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
PX4_WARN("gyro id changed, resetting IMU bias");
_imu_bias_reset_request = true;
}
}
src/modules/logger/logged_topics.cpp
+2 -2
View File
@@ -110,12 +110,12 @@ void LoggedTopics::add_default_topics()
add_topic_multi("distance_sensor", 1000);
add_topic_multi("optical_flow", 1000);
add_topic_multi("sensor_accel", 1000);
add_topic_multi("sensor_accel_status", 1000);
add_topic_multi("sensor_baro", 1000);
add_topic_multi("sensor_gyro", 1000);
add_topic_multi("sensor_gyro_status", 1000);
add_topic_multi("sensor_mag", 1000);
add_topic_multi("vehicle_gps_position", 1000);
add_topic_multi("vehicle_imu", 500);
add_topic_multi("vehicle_imu_status", 1000);
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
add_topic("actuator_controls_virtual_fw");
src/modules/mavlink/mavlink_messages.cpp
+48 -87
View File
@@ -84,12 +84,8 @@
#include <uORB/topics/orbit_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_accel_status.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_gyro_status.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/tecs_status.h>
@@ -106,6 +102,8 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -1062,12 +1060,11 @@ public:
unsigned get_size() override
{
return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 0};
uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 0};
uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 0};
uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 0};
// do not allow top copy this class
@@ -1080,28 +1077,25 @@ protected:
bool send(const hrt_abstime t) override
{
if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) {
if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) {
sensor_accel_integrated_s sensor_accel{};
_raw_accel_sub.copy(&sensor_accel);
sensor_gyro_integrated_s sensor_gyro{};
_raw_gyro_sub.copy(&sensor_gyro);
vehicle_imu_s imu{};
_raw_imu_sub.copy(&imu);
sensor_mag_s sensor_mag{};
_raw_mag_sub.copy(&sensor_mag);
mavlink_scaled_imu_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.time_boot_ms = imu.timestamp / 1000;
// Accelerometer in mG
const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
// Gyroscope in mrad/s
const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
@@ -1152,12 +1146,11 @@ public:
unsigned get_size() override
{
return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 1};
uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 1};
uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 1};
uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 1};
// do not allow top copy this class
@@ -1170,28 +1163,25 @@ protected:
bool send(const hrt_abstime t) override
{
if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) {
if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) {
sensor_accel_integrated_s sensor_accel{};
_raw_accel_sub.copy(&sensor_accel);
sensor_gyro_integrated_s sensor_gyro{};
_raw_gyro_sub.copy(&sensor_gyro);
vehicle_imu_s imu{};
_raw_imu_sub.copy(&imu);
sensor_mag_s sensor_mag{};
_raw_mag_sub.copy(&sensor_mag);
mavlink_scaled_imu2_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.time_boot_ms = imu.timestamp / 1000;
// Accelerometer in mG
const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
// Gyroscope in mrad/s
const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
@@ -1241,12 +1231,11 @@ public:
unsigned get_size() override
{
return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 2};
uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 2};
uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 2};
uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 2};
// do not allow top copy this class
@@ -1259,28 +1248,25 @@ protected:
bool send(const hrt_abstime t) override
{
if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) {
if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) {
sensor_accel_integrated_s sensor_accel{};
_raw_accel_sub.copy(&sensor_accel);
sensor_gyro_integrated_s sensor_gyro{};
_raw_gyro_sub.copy(&sensor_gyro);
vehicle_imu_s imu{};
_raw_imu_sub.copy(&imu);
sensor_mag_s sensor_mag{};
_raw_mag_sub.copy(&sensor_mag);
mavlink_scaled_imu3_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.time_boot_ms = imu.timestamp / 1000;
// Accelerometer in mG
const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
// Gyroscope in mrad/s
const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
@@ -2817,13 +2803,7 @@ public:
return size;
}
for (auto &x : _sensor_accel_status_sub) {
if (x.advertised()) {
return size;
}
}
for (auto &x : _sensor_gyro_status_sub) {
for (auto &x : _vehicle_imu_status_sub) {
if (x.advertised()) {
return size;
}
@@ -2835,16 +2815,10 @@ public:
private:
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _sensor_accel_status_sub[3] {
{ORB_ID(sensor_accel_status), 0},
{ORB_ID(sensor_accel_status), 1},
{ORB_ID(sensor_accel_status), 2},
};
uORB::Subscription _sensor_gyro_status_sub[3] {
{ORB_ID(sensor_gyro_status), 0},
{ORB_ID(sensor_gyro_status), 1},
{ORB_ID(sensor_gyro_status), 2},
uORB::Subscription _vehicle_imu_status_sub[3] {
{ORB_ID(vehicle_imu_status), 0},
{ORB_ID(vehicle_imu_status), 1},
{ORB_ID(vehicle_imu_status), 2},
};
/* do not allow top copying this class */
@@ -2859,10 +2833,10 @@ protected:
{
bool updated = _sensor_selection_sub.updated();
// check for sensor_accel_status update
// check for vehicle_imu_status update
if (!updated) {
for (int i = 0; i < 3; i++) {
if (_sensor_accel_status_sub[i].updated() || _sensor_gyro_status_sub[i].updated()) {
if (_vehicle_imu_status_sub[i].updated()) {
updated = true;
break;
}
@@ -2882,29 +2856,16 @@ protected:
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
// primary gyro coning and high frequency vibration metrics
if (sensor_selection.gyro_device_id != 0) {
for (auto &x : _sensor_gyro_status_sub) {
sensor_gyro_status_s status;
if (x.copy(&status)) {
if (status.device_id == sensor_selection.gyro_device_id) {
msg.vibration_x = status.coning_vibration;
msg.vibration_y = status.vibration_metric;
break;
}
}
}
}
// primary accel high frequency vibration metric
if (sensor_selection.accel_device_id != 0) {
for (auto &x : _sensor_accel_status_sub) {
sensor_accel_status_s status;
for (auto &x : _vehicle_imu_status_sub) {
vehicle_imu_status_s status;
if (x.copy(&status)) {
if (status.device_id == sensor_selection.accel_device_id) {
msg.vibration_z = status.vibration_metric;
if (status.accel_device_id == sensor_selection.accel_device_id) {
msg.vibration_x = status.gyro_coning_vibration;
msg.vibration_y = status.gyro_vibration_metric;
msg.vibration_z = status.accel_vibration_metric;
break;
}
}
@@ -2913,11 +2874,11 @@ protected:
// accel 0, 1, 2 cumulative clipping
for (int i = 0; i < 3; i++) {
sensor_accel_status_s acc_status;
vehicle_imu_status_s status;
if (_sensor_accel_status_sub[i].copy(&acc_status)) {
if (_vehicle_imu_status_sub[i].copy(&status)) {
const uint32_t clipping = acc_status.clipping[0] + acc_status.clipping[1] + acc_status.clipping[2];
const uint32_t clipping = status.accel_clipping[0] + status.accel_clipping[1] + status.accel_clipping[2];
switch (i) {
case 0:
src/modules/mavlink/mavlink_receiver.cpp
+6 -28
View File
@@ -2188,15 +2188,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* gyro */
{
if (_px4_gyro == nullptr) {
// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(2294028);
if (_px4_gyro == nullptr) {
PX4_ERR("PX4Gyroscope alloc failed");
} else {
_px4_gyro->set_update_rate(200); // TODO: measure actual
}
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(1311244);
}
if (_px4_gyro != nullptr) {
@@ -2208,15 +2201,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* accelerometer */
{
if (_px4_accel == nullptr) {
// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1311244);
if (_px4_accel == nullptr) {
PX4_ERR("PX4Accelerometer alloc failed");
} else {
_px4_accel->set_update_rate(200); // TODO: measure actual
}
}
if (_px4_accel != nullptr) {
@@ -2230,10 +2216,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
if (_px4_mag == nullptr) {
// 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
_px4_mag = new PX4Magnetometer(197388);
if (_px4_mag == nullptr) {
PX4_ERR("PX4Magnetometer alloc failed");
}
}
if (_px4_mag != nullptr) {
@@ -2247,10 +2229,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
if (_px4_baro == nullptr) {
// 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
_px4_baro = new PX4Barometer(6620172);
if (_px4_baro == nullptr) {
PX4_ERR("PX4Barometer alloc failed");
}
}
if (_px4_baro != nullptr) {
@@ -2629,7 +2607,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* accelerometer */
{
if (_px4_accel == nullptr) {
// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1311244);
if (_px4_accel == nullptr) {
@@ -2647,8 +2625,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* gyroscope */
{
if (_px4_gyro == nullptr) {
// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(2294028);
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(1311244);
if (_px4_gyro == nullptr) {
PX4_ERR("PX4Gyroscope alloc failed");
src/modules/sensors/CMakeLists.txt
-1
View File
@@ -41,7 +41,6 @@ add_subdirectory(vehicle_imu)
px4_add_module(
MODULE modules__sensors
MAIN sensors
PRIORITY "SCHED_PRIORITY_MAX-5"
SRCS
voted_sensors_update.cpp
sensors.cpp
src/modules/sensors/sensor_corrections/SensorCorrections.hpp
+2
View File
@@ -69,6 +69,8 @@ public:
void ParametersUpdate();
void SensorCorrectionsUpdate(bool force = false);
const matrix::Dcmf &getBoardRotation() const { return _board_rotation; }
private:
static constexpr int MAX_SENSOR_COUNT = 3;
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();
}
}
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+11 -7
View File
@@ -43,7 +43,7 @@ namespace sensors
VehicleAcceleration::VehicleAcceleration() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
_corrections(this, SensorCorrections::SensorType::Accelerometer)
{
_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
@@ -65,7 +65,10 @@ bool VehicleAcceleration::Start()
return false;
}
ScheduleNow();
if (!SensorSelectionUpdate(true)) {
ScheduleDelayed(10_ms);
}
return true;
}
@@ -198,6 +201,9 @@ void VehicleAcceleration::ParametersUpdate(bool force)
void VehicleAcceleration::Run()
{
// backup schedule
ScheduleDelayed(10_ms);
// update corrections first to set _selected_sensor
bool selection_updated = SensorSelectionUpdate();
@@ -259,11 +265,9 @@ void VehicleAcceleration::Run()
void VehicleAcceleration::PrintStatus()
{
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
_selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
_corrections.PrintStatus();
}
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+2 -2
View File
@@ -41,7 +41,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
@@ -54,7 +54,7 @@
namespace sensors
{
class VehicleAcceleration : public ModuleParams, public px4::WorkItem
class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem
{
public:
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
-1
View File
@@ -278,6 +278,5 @@ void VehicleAirData::PrintStatus()
PX4_INFO("selected barometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id, _selected_sensor_sub_index);
}
perf_print_counter(_cycle_perf);
_voter.print();
}
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+11 -7
View File
@@ -43,7 +43,7 @@ namespace sensors
VehicleAngularVelocity::VehicleAngularVelocity() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_corrections(this, SensorCorrections::SensorType::Gyroscope)
{
_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
@@ -68,7 +68,10 @@ bool VehicleAngularVelocity::Start()
return false;
}
ScheduleNow();
if (!SensorSelectionUpdate(true)) {
ScheduleDelayed(10_ms);
}
return true;
}
@@ -218,6 +221,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
void VehicleAngularVelocity::Run()
{
// backup schedule
ScheduleDelayed(10_ms);
// update corrections first to set _selected_sensor
bool selection_updated = SensorSelectionUpdate();
@@ -311,11 +317,9 @@ void VehicleAngularVelocity::Run()
void VehicleAngularVelocity::PrintStatus()
{
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
_selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
_corrections.PrintStatus();
}
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+2 -2
View File
@@ -42,7 +42,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
@@ -56,7 +56,7 @@
namespace sensors
{
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem
{
public:
src/modules/sensors/vehicle_imu/CMakeLists.txt
+2
View File
@@ -32,6 +32,8 @@
############################################################################
px4_add_library(vehicle_imu
Integrator.cpp
Integrator.hpp
VehicleIMU.cpp
VehicleIMU.hpp
)
src/modules/sensors/vehicle_imu/Integrator.cpp
+97
View File
@@ -0,0 +1,97 @@
/****************************************************************************
*
* Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "Integrator.hpp"
#include <drivers/drv_hrt.h>
using matrix::Vector3f;
bool Integrator::put(const hrt_abstime &timestamp, const Vector3f &val)
{
if ((_last_integration_time == 0) || (timestamp <= _last_integration_time)) {
/* this is the first item in the integrator */
_last_integration_time = timestamp;
_last_reset_time = timestamp;
_last_val = val;
return false;
}
// Use trapezoidal integration to calculate the delta integral
const float dt = static_cast<float>(timestamp - _last_integration_time) * 1e-6f;
const matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f;
_last_val = val;
_last_integration_time = timestamp;
_integrated_samples++;
// Calculate coning corrections if required
if (_coning_comp_on) {
// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
// following:
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
// Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
_beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f;
_last_delta_alpha = delta_alpha;
_last_alpha = _alpha;
}
// accumulate delta integrals
_alpha += delta_alpha;
return true;
}
bool Integrator::reset(Vector3f &integral, uint32_t &integral_dt)
{
if (integral_ready()) {
integral = Vector3f{_alpha};
_alpha.zero();
integral_dt = (_last_integration_time - _last_reset_time);
_last_reset_time = _last_integration_time;
_integrated_samples = 0;
// apply coning corrections if required
if (_coning_comp_on) {
integral += _beta;
_beta.zero();
_last_alpha.zero();
}
return true;
}
return false;
}
src/modules/sensors/vehicle_imu/Integrator.hpp
+123
View File
@@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Integrator.hpp
*
* A resettable integrator
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Julian Oes <julian@oes.ch>
*/
#pragma once
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
class Integrator
{
public:
Integrator(bool coning_compensation = false) : _coning_comp_on(coning_compensation) {}
~Integrator() = default;
/**
* Put an item into the integral.
*
* @param timestamp Timestamp of the current value.
* @param val Item to put.
* @return true if data was accepted and integrated.
*/
bool put(const uint64_t &timestamp, const matrix::Vector3f &val);
/**
* Put an item into the integral.
*
* @param timestamp Timestamp of the current value.
* @param val Item to put.
* @param integral Current integral in case the integrator did reset, else the value will not be modified
* @param integral_dt Get the dt in us of the current integration (only if reset).
* @return true if putting the item triggered an integral reset and the integral should be
* published.
*/
bool put(const uint64_t &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
{
return put(timestamp, val) && reset(integral, integral_dt);
}
/**
* Set reset interval during runtime. This won't reset the integrator.
*
* @param reset_interval New reset time interval for the integrator.
*/
void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval; }
/**
* Set required samples for reset. This won't reset the integrator.
*
* @param reset_samples New reset time interval for the integrator.
*/
void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; }
uint8_t get_reset_samples() const { return _reset_samples_min; }
/**
* Is the Integrator ready to reset?
*
* @return true if integrator has sufficient data (minimum interval & samples satisfied) to reset.
*/
bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_last_integration_time >= (_last_reset_time + _reset_interval_min)); }
/* Reset integrator and return current integral & integration time
*
* @param integral_dt Get the dt in us of the current integration.
* @return true if integral valid
*/
bool reset(matrix::Vector3f &integral, uint32_t &integral_dt);
private:
uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */
uint64_t _last_reset_time{0}; /**< last auto-announcement of integral value */
matrix::Vector3f _alpha{0.f, 0.f, 0.f}; /**< integrated value before coning corrections are applied */
matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */
matrix::Vector3f _beta{0.f, 0.f, 0.f}; /**< accumulated coning corrections */
matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */
matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */
uint32_t _reset_interval_min{1}; /**< the interval after which the content will be published and the integrator reset */
uint8_t _integrated_samples{0};
uint8_t _reset_samples_min{1};
const bool _coning_comp_on{false}; /**< true to turn on coning corrections */
};
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+254 -38
View File
@@ -35,20 +35,37 @@
#include <px4_platform_common/log.h>
#include <float.h>
using namespace matrix;
using namespace time_literals;
using math::constrain;
namespace sensors
{
VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
_sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index),
_sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
_sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index),
_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index),
_accel_corrections(this, SensorCorrections::SensorType::Accelerometer),
_gyro_corrections(this, SensorCorrections::SensorType::Gyroscope)
{
const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
_accel_integrator.set_reset_interval(configured_interval_us);
_accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH);
_sensor_accel_sub.set_required_updates(1);
_gyro_integrator.set_reset_interval(configured_interval_us);
_gyro_integrator.set_reset_samples(sensor_gyro_s::ORB_QUEUE_LENGTH);
_sensor_gyro_sub.set_required_updates(1);
// advertise immediately to ensure consistent ordering
_vehicle_imu_pub.advertise();
_vehicle_imu_status_pub.advertise();
}
VehicleIMU::~VehicleIMU()
@@ -61,14 +78,14 @@ bool VehicleIMU::Start()
// force initial updates
ParametersUpdate(true);
return _sensor_accel_integrated_sub.registerCallback() && _sensor_gyro_integrated_sub.registerCallback();
return _sensor_gyro_sub.registerCallback();
}
void VehicleIMU::Stop()
{
// clear all registered callbacks
_sensor_accel_integrated_sub.unregisterCallback();
_sensor_gyro_integrated_sub.unregisterCallback();
_sensor_accel_sub.unregisterCallback();
_sensor_gyro_sub.unregisterCallback();
Deinit();
}
@@ -85,52 +102,251 @@ void VehicleIMU::ParametersUpdate(bool force)
_accel_corrections.ParametersUpdate();
_gyro_corrections.ParametersUpdate();
// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), 50, 1000);
if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
_param_imu_integ_rate.set(imu_integration_rate_hz);
_param_imu_integ_rate.commit_no_notification();
}
}
}
bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample)
{
bool updated = false;
if ((intavg.timestamp_sample_last > 0) && (timestamp_sample > intavg.timestamp_sample_last)) {
intavg.interval_sum += (timestamp_sample - intavg.timestamp_sample_last);
intavg.interval_count++;
} else {
intavg.interval_sum = 0.f;
intavg.interval_count = 0.f;
}
intavg.timestamp_sample_last = timestamp_sample;
// periodically calculate sensor update rate
if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) {
const float sample_interval_avg = intavg.interval_sum / intavg.interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) {
// update if interval has changed by more than 1%
if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.01f) {
intavg.update_interval = sample_interval_avg;
updated = true;
}
}
// reset sample interval accumulator
intavg.timestamp_sample_last = 0;
}
return updated;
}
void VehicleIMU::Run()
{
if (_sensor_accel_integrated_sub.updated() && _sensor_gyro_integrated_sub.updated()) {
sensor_accel_integrated_s accel;
_sensor_accel_integrated_sub.copy(&accel);
_accel_corrections.set_device_id(accel.device_id);
// backup schedule
ScheduleDelayed(10_ms);
sensor_gyro_integrated_s gyro;
_sensor_gyro_integrated_sub.copy(&gyro);
ParametersUpdate();
_accel_corrections.SensorCorrectionsUpdate();
_gyro_corrections.SensorCorrectionsUpdate();
bool update_integrator_config = false;
// integrate queued gyro
sensor_gyro_s gyro;
while (!_gyro_integrator.integral_ready() && _sensor_gyro_sub.update(&gyro)) {
_gyro_corrections.set_device_id(gyro.device_id);
_gyro_error_count = gyro.error_count;
ParametersUpdate();
_accel_corrections.SensorCorrectionsUpdate();
_gyro_corrections.SensorCorrectionsUpdate();
const Vector3f gyro_corrected{_gyro_corrections.Correct(Vector3f{gyro.x, gyro.y, gyro.z})};
_gyro_integrator.put(gyro.timestamp_sample, gyro_corrected);
_last_timestamp_sample_gyro = gyro.timestamp_sample;
// delta angle: apply offsets, scale, and board rotation
const float gyro_dt = 1.e-6f * gyro.dt;
Vector3f delta_angle = _gyro_corrections.Correct(Vector3f{gyro.delta_angle} * gyro_dt) / gyro_dt;
// delta velocity: apply offsets, scale, and board rotation
const float accel_dt = 1.e-6f * accel.dt;
Vector3f delta_velocity = _accel_corrections.Correct(Vector3f{accel.delta_velocity} * accel_dt) / accel_dt;
// publich vehicle_imu
vehicle_imu_s imu;
imu.timestamp_sample = accel.timestamp_sample;
imu.accel_device_id = accel.device_id;
imu.gyro_device_id = gyro.device_id;
delta_angle.copyTo(imu.delta_angle);
delta_velocity.copyTo(imu.delta_velocity);
imu.dt = accel.dt;
//imu.clip_count = accel.clip_count;
imu.timestamp = hrt_absolute_time();
_vehicle_imu_pub.publish(imu);
// collect sample interval average for filters
if (UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
update_integrator_config = true;
}
}
// update accel, stopping once caught up to the last gyro sample
sensor_accel_s accel;
while (_sensor_accel_sub.update(&accel)) {
_accel_corrections.set_device_id(accel.device_id);
_accel_error_count = accel.error_count;
const Vector3f accel_corrected{_accel_corrections.Correct(Vector3f{accel.x, accel.y, accel.z})};
_accel_integrator.put(accel.timestamp_sample, accel_corrected);
_last_timestamp_sample_accel = accel.timestamp_sample;
// collect sample interval average for filters
if (UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
update_integrator_config = true;
}
if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
// rotate sensor clip counts into vehicle body frame
const Vector3f clipping{_accel_corrections.getBoardRotation() *
Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}};
// round to get reasonble clip counts per axis (after board rotation)
const uint8_t clip_x = roundf(fabsf(clipping(0)));
const uint8_t clip_y = roundf(fabsf(clipping(1)));
const uint8_t clip_z = roundf(fabsf(clipping(2)));
_delta_velocity_clipping_total[0] += clip_x;
_delta_velocity_clipping_total[1] += clip_y;
_delta_velocity_clipping_total[2] += clip_z;
if (clip_x > 0) {
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X;
}
if (clip_y > 0) {
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y;
}
if (clip_z > 0) {
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
}
}
// break once caught up to gyro
if (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval)) {
break;
}
}
// reconfigure integrators if calculated sensor intervals have changed
if (update_integrator_config) {
UpdateIntegratorConfiguration();
}
// publish if both accel & gyro integrators are ready
if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
uint32_t accel_integral_dt;
uint32_t gyro_integral_dt;
Vector3f delta_angle;
Vector3f delta_velocity;
if (_accel_integrator.reset(delta_velocity, accel_integral_dt)
&& _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
UpdateAccelVibrationMetrics(delta_velocity);
UpdateGyroVibrationMetrics(delta_angle);
// vehicle_imu_status
// publish first so that error counts are available synchronously if needed
vehicle_imu_status_s status;
status.accel_device_id = _accel_corrections.get_device_id();
status.gyro_device_id = _gyro_corrections.get_device_id();
status.accel_error_count = _accel_error_count;
status.gyro_error_count = _gyro_error_count;
status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
status.gyro_rate_hz = round(1e6f / _gyro_interval.update_interval);
status.accel_vibration_metric = _accel_vibration_metric;
status.gyro_vibration_metric = _gyro_vibration_metric;
status.gyro_coning_vibration = _gyro_coning_vibration;
status.accel_clipping[0] = _delta_velocity_clipping_total[0];
status.accel_clipping[1] = _delta_velocity_clipping_total[1];
status.accel_clipping[2] = _delta_velocity_clipping_total[2];
status.timestamp = hrt_absolute_time();
_vehicle_imu_status_pub.publish(status);
// publish vehicle_imu
vehicle_imu_s imu;
imu.timestamp_sample = _last_timestamp_sample_gyro;
imu.accel_device_id = _accel_corrections.get_device_id();
imu.gyro_device_id = _gyro_corrections.get_device_id();
delta_angle.copyTo(imu.delta_angle);
delta_velocity.copyTo(imu.delta_velocity);
imu.delta_angle_dt = gyro_integral_dt;
imu.delta_velocity_dt = accel_integral_dt;
imu.delta_velocity_clipping = _delta_velocity_clipping;
imu.timestamp = hrt_absolute_time();
_vehicle_imu_pub.publish(imu);
// reset clip counts
_delta_velocity_clipping = 0;
return;
}
}
}
void VehicleIMU::UpdateIntegratorConfiguration()
{
if ((_accel_interval.update_interval > 0) && (_gyro_interval.update_interval > 0)) {
const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
// determine number of sensor samples that will get closest to the desired integration interval
const uint8_t accel_integral_samples = constrain(roundf(configured_interval_us / _accel_interval.update_interval),
1.f, (float)sensor_accel_s::ORB_QUEUE_LENGTH);
const uint8_t gyro_integral_samples = constrain(roundf(configured_interval_us / _gyro_interval.update_interval),
1.f, (float)sensor_gyro_s::ORB_QUEUE_LENGTH);
// let the gyro set the configuration and scheduling
// accel integrator will be forced to reset when gyro integrator is ready
_gyro_integrator.set_reset_samples(gyro_integral_samples);
_accel_integrator.set_reset_samples(1);
// relaxed minimum integration time required
_accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval.update_interval));
_gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval.update_interval));
_sensor_accel_sub.set_required_updates(accel_integral_samples);
_sensor_gyro_sub.set_required_updates(gyro_integral_samples);
// run when there are enough new gyro samples, unregister accel
_sensor_accel_sub.unregisterCallback();
PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d",
_accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples);
}
}
void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &delta_velocity)
{
// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev;
_accel_vibration_metric = 0.99f * _accel_vibration_metric + 0.01f * delta_velocity_diff.norm();
_delta_velocity_prev = delta_velocity;
}
void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
{
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev;
_gyro_vibration_metric = 0.99f * _gyro_vibration_metric + 0.01f * delta_angle_diff.norm();
// Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
const Vector3f coning_metric = delta_angle % _delta_angle_prev;
_gyro_coning_vibration = 0.99f * _gyro_coning_vibration + 0.01f * coning_metric.norm();
_delta_angle_prev = delta_angle;
}
void VehicleIMU::PrintStatus()
{
PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_corrections.get_device_id(), _gyro_corrections.get_device_id());
PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us",
_accel_corrections.get_device_id(), (double)_accel_interval.update_interval,
_gyro_corrections.get_device_id(), (double)_gyro_interval.update_interval);
_accel_corrections.PrintStatus();
_gyro_corrections.PrintStatus();
}
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+47 -6
View File
@@ -33,6 +33,8 @@
#pragma once
#include "Integrator.hpp"
#include <sensor_corrections/SensorCorrections.hpp>
#include <lib/mathlib/math/Limits.hpp>
@@ -40,19 +42,20 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
namespace sensors
{
class VehicleIMU : public ModuleParams, public px4::WorkItem
class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem
{
public:
VehicleIMU() = delete;
@@ -69,13 +72,51 @@ private:
void ParametersUpdate(bool force = false);
void Run() override;
struct IntervalAverage {
hrt_abstime timestamp_sample_last{0};
float interval_sum{0.f};
float interval_count{0.f};
float update_interval{0.f};
};
bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample);
void UpdateIntegratorConfiguration();
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::SubscriptionCallbackWorkItem _sensor_accel_integrated_sub;
uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub;
uORB::SubscriptionCallbackWorkItem _sensor_accel_sub;
uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
SensorCorrections _accel_corrections;
SensorCorrections _gyro_corrections;
Integrator _accel_integrator{}; // 200 Hz default
Integrator _gyro_integrator{true}; // 200 Hz default, coning compensation enabled
hrt_abstime _last_timestamp_sample_accel{0};
hrt_abstime _last_timestamp_sample_gyro{0};
IntervalAverage _accel_interval{};
IntervalAverage _gyro_interval{};
uint32_t _accel_error_count{0};
uint32_t _gyro_error_count{0};
matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement
matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement
float _accel_vibration_metric{0.f}; // high frequency vibration level in the IMU delta velocity data (m/s)
float _gyro_vibration_metric{0.f}; // high frequency vibration level in the IMU delta angle data (rad)
float _gyro_coning_vibration{0.f}; // Level of coning vibration in the IMU delta angles (rad^2)
uint8_t _delta_velocity_clipping{0};
uint32_t _delta_velocity_clipping_total[3] {};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
)
};
} // namespace sensors
src/modules/sensors/vehicle_imu/imu_parameters.c
+1
View File
@@ -35,6 +35,7 @@
* IMU integration rate.
*
* The rate at which raw IMU data is integrated to produce delta angles and delta velocities.
* Recommended to set this to a multiple of the estimator update period (currently 10 ms for ekf2).
*
* @min 100
* @max 1000
src/modules/sensors/voted_sensors_update.cpp
+90 -155
View File
@@ -52,18 +52,14 @@ using namespace sensors;
using namespace matrix;
using math::radians;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
: ModuleParams(nullptr), _parameters(parameters), _hil_enabled(hil_enabled), _mag_compensator(this)
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
ModuleParams(nullptr),
_vehicle_imu_sub(vehicle_imu_sub),
_parameters(parameters),
_hil_enabled(hil_enabled),
_mag_compensator(this)
{
for (unsigned i = 0; i < 3; i++) {
_corrections.gyro_scale_0[i] = 1.0f;
_corrections.accel_scale_0[i] = 1.0f;
_corrections.gyro_scale_1[i] = 1.0f;
_corrections.accel_scale_1[i] = 1.0f;
_corrections.gyro_scale_2[i] = 1.0f;
_corrections.accel_scale_2[i] = 1.0f;
}
_mag.voter.set_timeout(300000);
_mag.voter.set_equal_value_threshold(1000);
@@ -463,162 +459,104 @@ void VotedSensorsUpdate::parametersUpdate()
}
void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
{
float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 };
float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 };
for (int uorb_index = 0; uorb_index < 3; uorb_index++) {
vehicle_imu_s imu_report;
for (int uorb_index = 0; uorb_index < _accel.subscription_count; uorb_index++) {
if (_accel.enabled[uorb_index] && _gyro.enabled[uorb_index] && _vehicle_imu_sub[uorb_index].update(&imu_report)) {
sensor_accel_integrated_s accel_report;
if (_accel.enabled[uorb_index] && _accel.subscription[uorb_index].update(&accel_report)) {
// copy corresponding vehicle_imu_status for accel & gyro error counts
vehicle_imu_status_s imu_status{};
_vehicle_imu_status_sub[uorb_index].copy(&imu_status);
// First publication with data
if (_accel.priority[uorb_index] == 0) {
_accel.priority[uorb_index] = _accel.subscription[uorb_index].get_priority();
}
_accel_device_id[uorb_index] = accel_report.device_id;
/*
* Correct the raw sensor data for scale factor errors
* and offsets due to temperature variation. It is assumed that any filtering of input
* data required is performed in the sensor driver, preferably before downsampling.
*/
// convert the delta velocities to an equivalent acceleration before application of corrections
const float dt_inv = 1.e6f / (float)accel_report.dt;
Vector3f accel_data = Vector3f{accel_report.delta_velocity} * dt_inv;
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
// apply temperature compensation
accel_data(0) = (accel_data(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
accel_data(1) = (accel_data(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
accel_data(2) = (accel_data(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
accel_data = _board_rotation * accel_data;
_last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0);
_last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1);
_last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2);
// record if there's any clipping per axis
_last_sensor_data[uorb_index].accelerometer_clipping = 0;
if (accel_report.clip_counter[0] > 0 || accel_report.clip_counter[1] > 0 || accel_report.clip_counter[2] > 0) {
const Vector3f sensor_clip_count{(float)accel_report.clip_counter[0], (float)accel_report.clip_counter[1], (float)accel_report.clip_counter[2]};
const Vector3f clipping{_board_rotation * sensor_clip_count};
static constexpr float CLIP_COUNT_THRESHOLD = 1.f;
if (fabsf(clipping(0)) >= CLIP_COUNT_THRESHOLD) {
_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_X;
}
if (fabsf(clipping(1)) >= CLIP_COUNT_THRESHOLD) {
_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Y;
}
if (fabsf(clipping(2)) >= CLIP_COUNT_THRESHOLD) {
_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Z;
}
}
_last_accel_timestamp[uorb_index] = accel_report.timestamp;
_accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
accel_report.error_count, _accel.priority[uorb_index]);
}
}
// find the best sensor
int best_index;
_accel.voter.get_best(hrt_absolute_time(), &best_index);
// write the best sensor data to the output variables
if (best_index >= 0) {
raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2));
raw.accelerometer_clipping = _last_sensor_data[best_index].accelerometer_clipping;
if (best_index != _accel.last_best_vote) {
_accel.last_best_vote = (uint8_t)best_index;
}
if (_selection.accel_device_id != _accel_device_id[best_index]) {
_selection_changed = true;
_selection.accel_device_id = _accel_device_id[best_index];
}
}
}
void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
{
float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 };
float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 };
for (int uorb_index = 0; uorb_index < _gyro.subscription_count; uorb_index++) {
sensor_gyro_integrated_s gyro_report;
if (_gyro.enabled[uorb_index] && _gyro.subscription[uorb_index].update(&gyro_report)) {
// First publication with data
if (_gyro.priority[uorb_index] == 0) {
_gyro.priority[uorb_index] = _gyro.subscription[uorb_index].get_priority();
}
_gyro_device_id[uorb_index] = gyro_report.device_id;
_accel_device_id[uorb_index] = imu_report.accel_device_id;
_gyro_device_id[uorb_index] = imu_report.gyro_device_id;
/*
* Correct the raw sensor data for scale factor errors
* and offsets due to temperature variation. It is assumed that any filtering of input
* data required is performed in the sensor driver, preferably before downsampling.
*/
// convert the delta velocities to an equivalent acceleration
const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt;
Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv;
// convert the delta angles to an equivalent angular rate before application of corrections
const float dt_inv = 1.e6f / (float)gyro_report.dt;
Vector3f gyro_rate = Vector3f{gyro_report.delta_angle} * dt_inv;
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
// apply temperature compensation
gyro_rate(0) = (gyro_rate(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
gyro_rate(1) = (gyro_rate(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
gyro_rate(2) = (gyro_rate(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
gyro_rate = _board_rotation * gyro_rate;
// convert the delta angles to an equivalent angular rate
const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt;
Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv;
_last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample;
_last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0);
_last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1);
_last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2);
_last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt;
_last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping;
_last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0);
_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
_last_sensor_data[uorb_index].timestamp = gyro_report.timestamp_sample;
_gyro.voter.put(uorb_index, gyro_report.timestamp, _last_sensor_data[uorb_index].gyro_rad,
gyro_report.error_count, _gyro.priority[uorb_index]);
_last_accel_timestamp[uorb_index] = imu_report.timestamp_sample;
_accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
imu_status.accel_error_count, _accel.priority[uorb_index]);
_gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad,
imu_status.gyro_error_count, _gyro.priority[uorb_index]);
}
}
// find the best sensor
int best_index;
_gyro.voter.get_best(hrt_absolute_time(), &best_index);
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);
checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
// write data for the best sensor to output variables
if (best_index >= 0) {
raw.timestamp = _last_sensor_data[best_index].timestamp;
raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
memcpy(&raw.gyro_rad, &_last_sensor_data[best_index].gyro_rad, sizeof(raw.gyro_rad));
if ((accel_best_index >= 0) && (gyro_best_index >= 0)) {
raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
sizeof(raw.accelerometer_m_s2));
memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad));
raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt;
raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping;
if (_gyro.last_best_vote != best_index) {
_gyro.last_best_vote = (uint8_t)best_index;
if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
_accel.last_best_vote = (uint8_t)accel_best_index;
_selection.accel_device_id = _accel_device_id[accel_best_index];
_selection_changed = true;
}
if (_selection.gyro_device_id != _gyro_device_id[best_index]) {
if ((_gyro.last_best_vote != gyro_best_index) || (_selection.gyro_device_id != _gyro_device_id[gyro_best_index])) {
_gyro.last_best_vote = (uint8_t)gyro_best_index;
_selection.gyro_device_id = _gyro_device_id[gyro_best_index];
_selection_changed = true;
_selection.gyro_device_id = _gyro_device_id[best_index];
// clear all registered callbacks
for (auto &sub : _vehicle_imu_sub) {
sub.unregisterCallback();
}
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
vehicle_imu_s report{};
if (_vehicle_imu_sub[i].copy(&report)) {
if ((report.gyro_device_id != 0) && (report.gyro_device_id == _gyro_device_id[gyro_best_index])) {
_vehicle_imu_sub[i].registerCallback();
}
}
}
}
}
}
@@ -655,6 +593,8 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
int best_index;
_mag.voter.get_best(hrt_absolute_time(), &best_index);
checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
if (best_index >= 0) {
magnetometer = _last_magnetometer[best_index];
_mag.last_best_vote = (uint8_t)best_index;
@@ -769,39 +709,34 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor
void VotedSensorsUpdate::printStatus()
{
PX4_INFO("gyro status:");
PX4_INFO("selected gyro: %d (%d)", _selection.gyro_device_id, _gyro.last_best_vote);
_gyro.voter.print();
PX4_INFO("accel status:");
PX4_INFO_RAW("\n");
PX4_INFO("selected accel: %d (%d)", _selection.accel_device_id, _accel.last_best_vote);
_accel.voter.print();
PX4_INFO("mag status:");
PX4_INFO_RAW("\n");
PX4_INFO("selected mag: %d (%d)", _selection.mag_device_id, _mag.last_best_vote);
_mag.voter.print();
}
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer)
{
_corrections_sub.update(&_corrections);
accelPoll(raw);
gyroPoll(raw);
imuPoll(raw);
magPoll(magnetometer);
// publish sensor selection if changed
if (_selection_changed) {
_selection.timestamp = hrt_absolute_time();
_sensor_selection_pub.publish(_selection);
_selection_changed = false;
// 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;
}
}
}
void VotedSensorsUpdate::checkFailover()
{
checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
}
void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
{
if (_last_accel_timestamp[_accel.last_best_vote]) {
src/modules/sensors/voted_sensors_update.h
+16 -27
View File
@@ -55,12 +55,12 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
@@ -81,7 +81,8 @@ public:
* @param parameters parameter values. These do not have to be initialized when constructing this object.
* Only when calling init(), they have to be initialized.
*/
VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled);
VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
/**
* initialize subscriptions etc.
@@ -113,13 +114,6 @@ public:
*/
void setRelativeTimestamps(sensor_combined_s &raw);
/**
* check if a failover event occured. if so, report it.
*/
void checkFailover();
int bestGyroID() const { return _gyro_device_id[_gyro.last_best_vote]; }
/**
* Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
*/
@@ -166,20 +160,12 @@ private:
void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max);
/**
* Poll the accelerometer for updated data.
* Poll IMU for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void accelPoll(sensor_combined_s &raw);
/**
* Poll the gyro for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void gyroPoll(sensor_combined_s &raw);
void imuPoll(sensor_combined_s &raw);
/**
* Poll the magnetometer for updated data.
@@ -195,8 +181,8 @@ private:
*/
bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type);
SensorData _accel{ORB_ID::sensor_accel_integrated};
SensorData _gyro{ORB_ID::sensor_gyro_integrated};
SensorData _accel{ORB_ID::sensor_accel};
SensorData _gyro{ORB_ID::sensor_gyro};
SensorData _mag{ORB_ID::sensor_mag};
orb_advert_t _mavlink_log_pub{nullptr};
@@ -204,8 +190,12 @@ private:
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */
/* sensor thermal compensation */
uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3];
uORB::Subscription _vehicle_imu_status_sub[ACCEL_COUNT_MAX] {
{ORB_ID(vehicle_imu_status), 0},
{ORB_ID(vehicle_imu_status), 1},
{ORB_ID(vehicle_imu_status), 2},
};
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
@@ -216,7 +206,7 @@ private:
const Parameters &_parameters;
const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */
bool _selection_changed{false}; /**< true when a sensor selection has changed and not been published */
bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */
float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */
float _gyro_diff[3][2] {}; /**< filtered gyro differences between IMU uinits (rad/s) */
@@ -231,7 +221,6 @@ private:
uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */
sensor_correction_s _corrections {}; /**< struct containing the sensor corrections to be published to the uORB */
sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */
subsystem_info_s _info {}; /**< subsystem info publication */
};
src/modules/sih/sih.cpp
-2
View File
@@ -95,8 +95,6 @@ Sih::Sih() :
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": execution")),
_sampling_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sampling"))
{
_px4_accel.set_update_rate(LOOP_INTERVAL);
_px4_gyro.set_update_rate(LOOP_INTERVAL);
}
void Sih::run()
src/modules/sih/sih.hpp
+2 -2
View File
@@ -98,8 +98,8 @@ private:
void parameters_updated();
// simulated sensor instances
PX4Accelerometer _px4_accel{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{ 2294028, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
PX4Accelerometer _px4_accel{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{ 197388, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
src/modules/simulator/simulator.cpp
+1 -1
View File
@@ -128,7 +128,7 @@ int simulator_main(int argc, char *argv[])
g_sim_task = px4_task_spawn_cmd("simulator",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
Simulator::start,
argv);
src/modules/simulator/simulator.h
+2 -5
View File
@@ -135,9 +135,6 @@ public:
private:
Simulator() : ModuleParams(nullptr)
{
// current default
_px4_accel.set_update_rate(250);
_px4_gyro.set_update_rate(250);
}
~Simulator()
@@ -162,8 +159,8 @@ private:
static Simulator *_instance;
// simulated sensor instances
PX4Accelerometer _px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{2294028, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
PX4Accelerometer _px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{6620172, ORB_PRIO_DEFAULT}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
src/modules/simulator/simulator_mavlink.cpp
+5 -5
View File
@@ -205,16 +205,16 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
}
}
// gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) {
_px4_gyro.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
}
// accel
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) {
_px4_accel.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
}
// gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) {
_px4_gyro.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
}
// magnetometer
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) {
_px4_mag.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
src/modules/uORB/Subscription.hpp
+8 -5
View File
@@ -42,6 +42,7 @@
#include <uORB/topics/uORBTopics.hpp>
#include <px4_platform_common/defines.h>
#include <lib/mathlib/mathlib.h>
#include "uORBDeviceNode.hpp"
#include "uORBManager.hpp"
@@ -124,17 +125,19 @@ public:
*/
bool copy(void *dst) { return advertised() ? _node->copy(dst, _last_generation) : false; }
uint8_t get_instance() const { return _instance; }
orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
ORB_PRIO get_priority() { return advertised() ? _node->get_priority() : ORB_PRIO_UNINITIALIZED; }
uint8_t get_instance() const { return _instance; }
unsigned get_last_generation() const { return _last_generation; }
ORB_PRIO get_priority() { return advertised() ? _node->get_priority() : ORB_PRIO_UNINITIALIZED; }
orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
protected:
friend class SubscriptionCallback;
friend class SubscriptionCallbackWorkItem;
DeviceNode *get_node() { return _node; }
DeviceNode *get_node() { return _node; }
DeviceNode *_node{nullptr};
DeviceNode *_node{nullptr};
unsigned _last_generation{0}; /**< last generation the subscriber has seen */
src/modules/uORB/SubscriptionCallback.hpp
+19 -3
View File
@@ -127,14 +127,30 @@ public:
void call() override
{
// schedule immediately if no interval, otherwise check time elapsed
if ((_interval_us == 0) || (hrt_elapsed_time_atomic(&_last_update) >= _interval_us)) {
_work_item->ScheduleNow();
// schedule immediately if updated (queue depth or subscription interval)
if ((_required_updates == 0)
|| (_subscription.get_node()->published_message_count() >= (_subscription.get_last_generation() + _required_updates))) {
if (updated()) {
_work_item->ScheduleNow();
}
}
}
/**
* Optionally limit callback until more samples are available.
*
* @param required_updates Number of queued updates required before a callback can be called.
*/
void set_required_updates(uint8_t required_updates)
{
// TODO: constrain to queue depth?
_required_updates = required_updates;
}
private:
px4::WorkItem *_work_item;
uint8_t _required_updates{0};
};
} // namespace uORB