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

sensor accel/gyro message cleanup

Browse Source 
- split out integrated data into new standalone messages (sensor_accel_integrated and sensor_gyro_integrated)
 - publish sensor_gyro at full rate and remove sensor_gyro_control
 - limit sensor status publications to 10 Hz
 - remove unused accel/gyro raw ADC fields
 - add device IDs to sensor_bias and sensor_correction
    - vehicle_angular_velocity/vehicle_acceleration: check device ids before using bias and corrections
This commit is contained in:
Daniel Agar
2020-01-18 01:15:00 -05:00
committed by GitHub
parent 1d932f6ec9
commit bb465ca5b7
34 changed files with 696 additions and 756 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/ekf2_main.cpp
View File

@@ -1472,6 +1472,10 @@ void Ekf2::Run()
bias.timestamp = now;
bias.gyro_device_id = _sensor_selection.gyro_device_id;
bias.accel_device_id = _sensor_selection.accel_device_id;
bias.mag_device_id = _sensor_selection.mag_device_id;
// In-run bias estimates
_ekf.get_gyro_bias(bias.gyro_bias);
_ekf.get_accel_bias(bias.accel_bias);

1
src/modules/logger/logged_topics.cpp
View File

@@ -69,6 +69,7 @@ void LoggedTopics::add_default_topics()
add_topic("position_setpoint_triplet", 200);
add_topic("radio_status");
add_topic("rate_ctrl_status", 200);
add_topic("sensor_bias", 1000);
add_topic("sensor_combined", 100);
add_topic("sensor_correction", 1000);
add_topic("sensor_preflight", 200);

144
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -60,13 +60,14 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/camera_capture.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_status.h>
@@ -75,18 +76,22 @@
#include <uORB/topics/input_rc.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/optical_flow.h>
#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_combined.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -97,21 +102,16 @@
#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_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/uORB.h>
using matrix::Vector3f;
using matrix::wrap_2pi;
static uint16_t cm_uint16_from_m_float(float m);
@@ -952,8 +952,8 @@ private:
protected:
explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 0)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 0)),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 0)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 0)),
_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 0)),
_raw_accel_time(0),
_raw_gyro_time(0),
@@ -962,9 +962,9 @@ protected:
bool send(const hrt_abstime t) override
{
sensor_accel_s sensor_accel = {};
sensor_gyro_s sensor_gyro = {};
sensor_mag_s sensor_mag = {};
sensor_accel_integrated_s sensor_accel{};
sensor_gyro_integrated_s sensor_gyro{};
sensor_mag_s sensor_mag{};
bool updated = false;
updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@@ -972,19 +972,27 @@ protected:
updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
if (updated) {
mavlink_scaled_imu_t msg = {};
mavlink_scaled_imu_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = sensor_mag.x_raw; // [milli tesla]
msg.ymag = sensor_mag.y_raw; // [milli tesla]
msg.zmag = sensor_mag.z_raw; // [milli tesla]
// 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;
// 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;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
msg.zacc = (int16_t)accel(2);
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
mavlink_msg_scaled_imu_send_struct(_mavlink->get_channel(), &msg);
@@ -1044,8 +1052,8 @@ private:
protected:
explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 1)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 1)),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 1)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 1)),
_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 1)),
_raw_accel_time(0),
_raw_gyro_time(0),
@@ -1054,9 +1062,9 @@ protected:
bool send(const hrt_abstime t) override
{
sensor_accel_s sensor_accel = {};
sensor_gyro_s sensor_gyro = {};
sensor_mag_s sensor_mag = {};
sensor_accel_integrated_s sensor_accel{};
sensor_gyro_integrated_s sensor_gyro{};
sensor_mag_s sensor_mag{};
bool updated = false;
updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@@ -1064,19 +1072,27 @@ protected:
updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
if (updated) {
mavlink_scaled_imu2_t msg = {};
mavlink_scaled_imu2_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = sensor_mag.x_raw; // [milli tesla]
msg.ymag = sensor_mag.y_raw; // [milli tesla]
msg.zmag = sensor_mag.z_raw; // [milli tesla]
// 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;
// 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;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
msg.zacc = (int16_t)accel(2);
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
mavlink_msg_scaled_imu2_send_struct(_mavlink->get_channel(), &msg);
@@ -1135,8 +1151,8 @@ private:
protected:
explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 2)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 2)),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 2)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 2)),
_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 2)),
_raw_accel_time(0),
_raw_gyro_time(0),
@@ -1145,9 +1161,9 @@ protected:
bool send(const hrt_abstime t) override
{
sensor_accel_s sensor_accel = {};
sensor_gyro_s sensor_gyro = {};
sensor_mag_s sensor_mag = {};
sensor_accel_integrated_s sensor_accel{};
sensor_gyro_integrated_s sensor_gyro{};
sensor_mag_s sensor_mag{};
bool updated = false;
updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@@ -1155,19 +1171,27 @@ protected:
updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
if (updated) {
mavlink_scaled_imu3_t msg = {};
mavlink_scaled_imu3_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = sensor_mag.x_raw; // [milli tesla]
msg.ymag = sensor_mag.y_raw; // [milli tesla]
msg.zmag = sensor_mag.z_raw; // [milli tesla]
// 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;
// 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;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
msg.zacc = (int16_t)accel(2);
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
mavlink_msg_scaled_imu3_send_struct(_mavlink->get_channel(), &msg);

12
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -2088,9 +2088,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
sensor_gyro_s gyro{};
gyro.timestamp = timestamp;
gyro.x_raw = imu.xgyro * 1000.0f;
gyro.y_raw = imu.ygyro * 1000.0f;
gyro.z_raw = imu.zgyro * 1000.0f;
gyro.x = imu.xgyro;
gyro.y = imu.ygyro;
gyro.z = imu.zgyro;
@@ -2104,9 +2101,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
sensor_accel_s accel{};
accel.timestamp = timestamp;
accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f);
accel.y_raw = imu.yacc / (CONSTANTS_ONE_G / 1000.0f);
accel.z_raw = imu.zacc / (CONSTANTS_ONE_G / 1000.0f);
accel.x = imu.xacc;
accel.y = imu.yacc;
accel.z = imu.zacc;
@@ -2507,9 +2501,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
sensor_accel_s accel{};
accel.timestamp = timestamp;
accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
accel.x = hil_state.xacc;
accel.y = hil_state.yacc;
accel.z = hil_state.zacc;
@@ -2523,9 +2514,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
sensor_gyro_s gyro{};
gyro.timestamp = timestamp;
gyro.x_raw = hil_state.rollspeed * 1e3f;
gyro.y_raw = hil_state.pitchspeed * 1e3f;
gyro.z_raw = hil_state.yawspeed * 1e3f;
gyro.x = hil_state.rollspeed;
gyro.y = hil_state.pitchspeed;
gyro.z = hil_state.yawspeed;

1
src/modules/sensors/vehicle_acceleration/CMakeLists.txt
View File

@@ -33,5 +33,6 @@
px4_add_library(vehicle_acceleration
VehicleAcceleration.cpp
VehicleAcceleration.hpp
)
target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue)

138
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@@ -49,26 +49,18 @@ VehicleAcceleration::~VehicleAcceleration()
Stop();
}
bool
VehicleAcceleration::Start()
bool VehicleAcceleration::Start()
{
// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset.zero();
_bias.zero();
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
_sensor_selection_sub.registerCallback();
return SensorCorrectionsUpdate(true);
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
}
void
VehicleAcceleration::Stop()
void VehicleAcceleration::Stop()
{
Deinit();
@@ -80,21 +72,23 @@ VehicleAcceleration::Stop()
_sensor_selection_sub.unregisterCallback();
}
void
VehicleAcceleration::SensorBiasUpdate(bool force)
void VehicleAcceleration::SensorBiasUpdate(bool force)
{
if (_sensor_bias_sub.updated() || force) {
sensor_bias_s bias;
if (_sensor_bias_sub.copy(&bias)) {
// TODO: should be checking device ID
_bias = Vector3f{bias.accel_bias};
if (bias.accel_device_id == _selected_sensor_device_id) {
_bias = Vector3f{bias.accel_bias};
} else {
_bias.zero();
}
}
}
}
bool
VehicleAcceleration::SensorCorrectionsUpdate(bool force)
void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
@@ -102,49 +96,82 @@ VehicleAcceleration::SensorCorrectionsUpdate(bool force)
sensor_correction_s corrections{};
_sensor_correction_sub.copy(&corrections);
// TODO: should be checking device ID
if (_selected_sensor == 0) {
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.accel_device_ids[i] == _selected_sensor_device_id) {
_corrections_selected_instance = i;
}
}
}
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.accel_offset_0};
_scale = Vector3f{corrections.accel_scale_0};
} else if (_selected_sensor == 1) {
break;
case 1:
_offset = Vector3f{corrections.accel_offset_1};
_scale = Vector3f{corrections.accel_scale_1};
} else if (_selected_sensor == 2) {
break;
case 2:
_offset = Vector3f{corrections.accel_offset_2};
_scale = Vector3f{corrections.accel_scale_2};
} else {
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
}
}
}
// update the latest sensor selection
if ((_selected_sensor != corrections.selected_accel_instance) || force) {
if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
bool VehicleAcceleration::SensorSelectionUpdate(bool force)
{
if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
const int sensor_new = corrections.selected_accel_instance;
if (_selected_sensor_device_id != sensor_selection.accel_device_id) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
if (_sensor_sub[sensor_new].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
_selected_sensor = sensor_new;
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_accel_s report{};
_sensor_sub[i].copy(&report);
return true;
if ((report.device_id != 0) && (report.device_id == sensor_selection.accel_device_id)) {
if (_sensor_sub[i].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
// record selected sensor (array index)
_selected_sensor_sub_index = i;
_selected_sensor_device_id = sensor_selection.accel_device_id;
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
return true;
}
}
}
PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id);
_selected_sensor_device_id = 0;
_selected_sensor_sub_index = 0;
}
}
return false;
}
void
VehicleAcceleration::ParametersUpdate(bool force)
void VehicleAcceleration::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
@@ -167,20 +194,19 @@ VehicleAcceleration::ParametersUpdate(bool force)
}
}
void
VehicleAcceleration::Run()
void VehicleAcceleration::Run()
{
// update corrections first to set _selected_sensor
bool sensor_select_update = SensorCorrectionsUpdate();
bool sensor_select_update = SensorSelectionUpdate();
SensorCorrectionsUpdate(sensor_select_update);
SensorBiasUpdate(sensor_select_update);
ParametersUpdate();
if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_accel_s sensor_data;
_sensor_sub[_selected_sensor].copy(&sensor_data);
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
@@ -192,8 +218,10 @@ VehicleAcceleration::Run()
// correct for in-run bias errors
accel -= _bias;
vehicle_acceleration_s out{};
out.timestamp_sample = sensor_data.timestamp;
// publish
vehicle_acceleration_s out;
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
@@ -201,8 +229,10 @@ VehicleAcceleration::Run()
}
}
void
VehicleAcceleration::PrintStatus()
void VehicleAcceleration::PrintStatus()
{
PX4_INFO("selected sensor: %d", _selected_sensor);
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("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
}

21
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View File

@@ -56,7 +56,7 @@ class VehicleAcceleration : public ModuleParams, public px4::WorkItem
public:
VehicleAcceleration();
virtual ~VehicleAcceleration();
~VehicleAcceleration() override;
void Run() override;
@@ -66,10 +66,10 @@ public:
void PrintStatus();
private:
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
bool SensorCorrectionsUpdate(bool force = false);
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
@@ -96,10 +96,11 @@ private:
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _offset;
matrix::Vector3f _scale;
matrix::Vector3f _bias;
uint8_t _selected_sensor{0};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
matrix::Vector3f _bias{0.f, 0.f, 0.f};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
};

1
src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt
View File

@@ -33,5 +33,6 @@
px4_add_library(vehicle_angular_velocity
VehicleAngularVelocity.cpp
VehicleAngularVelocity.hpp
)
target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue)

225
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@@ -49,26 +49,18 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
Stop();
}
bool
VehicleAngularVelocity::Start()
bool VehicleAngularVelocity::Start()
{
// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset.zero();
_bias.zero();
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
_sensor_selection_sub.registerCallback();
return SensorCorrectionsUpdate(true);
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
}
void
VehicleAngularVelocity::Stop()
void VehicleAngularVelocity::Stop()
{
Deinit();
@@ -80,110 +72,106 @@ VehicleAngularVelocity::Stop()
_sensor_selection_sub.unregisterCallback();
}
void
VehicleAngularVelocity::SensorBiasUpdate(bool force)
void VehicleAngularVelocity::SensorBiasUpdate(bool force)
{
if (_sensor_bias_sub.updated() || force) {
sensor_bias_s bias;
if (_sensor_bias_sub.copy(&bias)) {
// TODO: should be checking device ID
_bias = Vector3f{bias.gyro_bias};
if (bias.gyro_device_id == _selected_sensor_device_id) {
_bias = Vector3f{bias.gyro_bias};
} else {
_bias.zero();
}
}
}
}
bool
VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
{
if (_sensor_selection_sub.updated() || force) {
sensor_selection_s sensor_selection;
if (_sensor_selection_sub.copy(&sensor_selection)) {
if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
_selected_sensor_device_id = sensor_selection.gyro_device_id;
force = true;
}
}
}
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections{};
_sensor_correction_sub.copy(&corrections);
// TODO: should be checking device ID
if (_selected_sensor == 0) {
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_gyro_instance) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.gyro_device_ids[i] == _selected_sensor_device_id) {
_corrections_selected_instance = i;
}
}
}
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.gyro_offset_0};
_scale = Vector3f{corrections.gyro_scale_0};
} else if (_selected_sensor == 1) {
break;
case 1:
_offset = Vector3f{corrections.gyro_offset_1};
_scale = Vector3f{corrections.gyro_scale_1};
} else if (_selected_sensor == 2) {
break;
case 2:
_offset = Vector3f{corrections.gyro_offset_2};
_scale = Vector3f{corrections.gyro_scale_2};
} else {
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
}
}
}
// update the latest sensor selection
if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
// clear all registered callbacks
for (auto &sub : _sensor_control_sub) {
sub.unregisterCallback();
}
bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
{
if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
const int sensor_new = corrections.selected_gyro_instance;
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_gyro_s report{};
_sensor_sub[i].copy(&report);
// subscribe to sensor_gyro_control if available
// currently not all drivers (eg df_*) provide sensor_gyro_control
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_gyro_control_s report{};
_sensor_control_sub[i].copy(&report);
if ((report.device_id != 0) && (report.device_id == sensor_selection.gyro_device_id)) {
if (_sensor_sub[i].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) {
if (_sensor_control_sub[i].registerCallback()) {
PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i);
_selected_sensor_control = i;
// record selected sensor (array index)
_selected_sensor_sub_index = i;
_selected_sensor_device_id = sensor_selection.gyro_device_id;
_sensor_control_available = true;
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
// record selected sensor (sensor_gyro orb index)
_selected_sensor = sensor_new;
return true;
}
return true;
}
}
// otherwise fallback to using sensor_gyro (legacy that will be removed)
_sensor_control_available = false;
if (_sensor_sub[sensor_new].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
_selected_sensor = sensor_new;
return true;
}
}
PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id);
_selected_sensor_device_id = 0;
_selected_sensor_sub_index = 0;
}
}
return false;
}
void
VehicleAngularVelocity::ParametersUpdate(bool force)
void VehicleAngularVelocity::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
@@ -206,78 +194,45 @@ VehicleAngularVelocity::ParametersUpdate(bool force)
}
}
void
VehicleAngularVelocity::Run()
void VehicleAngularVelocity::Run()
{
// update corrections first to set _selected_sensor
bool sensor_select_update = SensorCorrectionsUpdate();
bool sensor_select_update = SensorSelectionUpdate();
SensorCorrectionsUpdate(sensor_select_update);
SensorBiasUpdate(sensor_select_update);
ParametersUpdate();
if (_sensor_control_available) {
// using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control
if (_sensor_control_sub[_selected_sensor].updated() || sensor_select_update) {
sensor_gyro_control_s sensor_data;
_sensor_control_sub[_selected_sensor].copy(&sensor_data);
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_gyro_s sensor_data;
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// get the sensor data and correct for thermal errors (apply offsets and scale)
Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates -= _bias;
// correct for in-run bias errors
rates -= _bias;
vehicle_angular_velocity_s angular_velocity;
angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
// publish
vehicle_angular_velocity_s out;
_vehicle_angular_velocity_pub.publish(angular_velocity);
}
out.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
} else {
// otherwise fallback to using sensor_gyro (legacy that will be removed)
if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
sensor_gyro_s sensor_data;
_sensor_sub[_selected_sensor].copy(&sensor_data);
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates -= _bias;
vehicle_angular_velocity_s angular_velocity;
angular_velocity.timestamp_sample = sensor_data.timestamp;
rates.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(angular_velocity);
}
_vehicle_angular_velocity_pub.publish(out);
}
}
void
VehicleAngularVelocity::PrintStatus()
void VehicleAngularVelocity::PrintStatus()
{
PX4_INFO("selected sensor: %d", _selected_sensor);
if (_selected_sensor_device_id != 0) {
PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control);
} else {
PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor);
}
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("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
}

30
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@@ -49,7 +49,6 @@
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_control.h>
#include <uORB/topics/vehicle_angular_velocity.h>
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@@ -57,7 +56,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
public:
VehicleAngularVelocity();
virtual ~VehicleAngularVelocity();
~VehicleAngularVelocity() override;
void Run() override;
@@ -67,10 +66,10 @@ public:
void PrintStatus();
private:
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
bool SensorCorrectionsUpdate(bool force = false);
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
@@ -89,28 +88,19 @@ private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */
{this, ORB_ID(sensor_gyro), 0},
{this, ORB_ID(sensor_gyro), 1},
{this, ORB_ID(sensor_gyro), 2}
};
uORB::SubscriptionCallbackWorkItem _sensor_control_sub[MAX_SENSOR_COUNT] { /**< sensor control data subscription */
{this, ORB_ID(sensor_gyro_control), 0},
{this, ORB_ID(sensor_gyro_control), 1},
{this, ORB_ID(sensor_gyro_control), 2}
};
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _offset;
matrix::Vector3f _scale;
matrix::Vector3f _bias;
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor{0};
uint8_t _selected_sensor_control{0};
bool _sensor_control_available{false};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
};

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

@@ -92,9 +92,9 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
void VotedSensorsUpdate::initializeSensors()
{
initSensorClass(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
initSensorClass(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX);
initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX);
initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
}
@@ -142,7 +142,7 @@ void VotedSensorsUpdate::parametersUpdate()
/* gyro */
for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_gyro_s> report{ORB_ID(sensor_gyro), topic_instance};
uORB::SubscriptionData<sensor_gyro_integrated_s> report{ORB_ID(sensor_gyro_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
@@ -151,10 +151,11 @@ void VotedSensorsUpdate::parametersUpdate()
topic_instance);
_corrections.gyro_mapping[topic_instance] = 0;
_corrections.gyro_device_ids[topic_instance] = 0;
} else {
_corrections.gyro_mapping[topic_instance] = temp;
_corrections.gyro_device_ids[topic_instance] = report.get().device_id;
}
}
@@ -162,7 +163,7 @@ void VotedSensorsUpdate::parametersUpdate()
/* accel */
for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_accel_s> report{ORB_ID(sensor_accel), topic_instance};
uORB::SubscriptionData<sensor_accel_integrated_s> report{ORB_ID(sensor_accel_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
@@ -171,10 +172,11 @@ void VotedSensorsUpdate::parametersUpdate()
topic_instance);
_corrections.accel_mapping[topic_instance] = 0;
_corrections.accel_device_ids[topic_instance] = 0;
} else {
_corrections.accel_mapping[topic_instance] = temp;
_corrections.accel_device_ids[topic_instance] = report.get().device_id;
}
}
@@ -191,10 +193,11 @@ void VotedSensorsUpdate::parametersUpdate()
topic_instance);
_corrections.baro_mapping[topic_instance] = 0;
_corrections.baro_device_ids[topic_instance] = 0;
} else {
_corrections.baro_mapping[topic_instance] = temp;
_corrections.baro_device_ids[topic_instance] = report.get().device_id;
}
}
@@ -565,9 +568,9 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
orb_check(_accel.subscription[uorb_index], &accel_updated);
if (accel_updated) {
sensor_accel_s accel_report;
sensor_accel_integrated_s accel_report;
int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
int ret = orb_copy(ORB_ID(sensor_accel_integrated), _accel.subscription[uorb_index], &accel_report);
if (ret != PX4_OK || accel_report.timestamp == 0) {
continue; //ignore invalid data
@@ -586,40 +589,17 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
_accel_device_id[uorb_index] = accel_report.device_id;
Vector3f accel_data;
/*
* 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.
*/
if (accel_report.integral_dt != 0) {
/*
* Using data that has been integrated in the driver before downsampling is preferred
* becasue it reduces aliasing errors. 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;
// convert the delta velocities to an equivalent acceleration before application of corrections
float dt_inv = 1.e6f / accel_report.integral_dt;
accel_data = Vector3f(accel_report.x_integral * dt_inv,
accel_report.y_integral * dt_inv,
accel_report.z_integral * dt_inv);
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt;
} else {
// using the value instead of the integral (the integral is the prefered choice)
// Correct each sensor for temperature effects
// Filtering and/or downsampling of temperature should be performed in the driver layer
accel_data = Vector3f(accel_report.x, accel_report.y, accel_report.z);
// handle the cse where this is our first output
if (_last_accel_timestamp[uorb_index] == 0) {
_last_accel_timestamp[uorb_index] = accel_report.timestamp - 1000;
}
// approximate the delta time using the difference in accel data time stamps
_last_sensor_data[uorb_index].accelerometer_integral_dt =
(accel_report.timestamp - _last_accel_timestamp[uorb_index]);
}
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
@@ -672,9 +652,9 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
orb_check(_gyro.subscription[uorb_index], &gyro_updated);
if (gyro_updated) {
sensor_gyro_s gyro_report;
sensor_gyro_integrated_s gyro_report;
int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
int ret = orb_copy(ORB_ID(sensor_gyro_integrated), _gyro.subscription[uorb_index], &gyro_report);
if (ret != PX4_OK || gyro_report.timestamp == 0) {
continue; //ignore invalid data
@@ -693,40 +673,17 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
_gyro_device_id[uorb_index] = gyro_report.device_id;
Vector3f gyro_rate;
/*
* 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.
*/
if (gyro_report.integral_dt != 0) {
/*
* Using data that has been integrated in the driver before downsampling is preferred
* becasue it reduces aliasing errors. 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 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;
// convert the delta angles to an equivalent angular rate before application of corrections
float dt_inv = 1.e6f / gyro_report.integral_dt;
gyro_rate = Vector3f(gyro_report.x_integral * dt_inv,
gyro_report.y_integral * dt_inv,
gyro_report.z_integral * dt_inv);
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt;
} else {
//using the value instead of the integral (the integral is the prefered choice)
// Correct each sensor for temperature effects
// Filtering and/or downsampling of temperature should be performed in the driver layer
gyro_rate = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
// handle the case where this is our first output
if (_last_sensor_data[uorb_index].timestamp == 0) {
_last_sensor_data[uorb_index].timestamp = gyro_report.timestamp - 1000;
}
// approximate the delta time using the difference in gyro data time stamps
_last_sensor_data[uorb_index].gyro_integral_dt =
(gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp);
}
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,

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

@@ -54,9 +54,11 @@
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_accel_integrated.h>
#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_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>