fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 15:40:35 +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/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: