fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_imu: incremental step towards multi-EKF

Browse Source
This commit is contained in:
Daniel Agar 2020-01-21 16:47:38 -05:00
parent 0e90448e52
commit 697dbfb9f8
13 changed files with 611 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -144,6 +144,7 @@ set(msg_files
vehicle_control_mode.msg
vehicle_global_position.msg
vehicle_gps_position.msg
vehicle_imu.msg
vehicle_land_detected.msg
vehicle_local_position.msg
vehicle_local_position_setpoint.msg

2
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -275,6 +275,8 @@ rtps:
id: 122
- msg: sensor_gyro_integrated
id: 123
- msg: vehicle_imu
id: 124
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

15
msg/vehicle_imu.msg Normal file
View File

@ -0,0 +1,15 @@
# IMU readings in SI-unit form.
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint32 accel_device_id # Accelerometer unique device ID for the sensor that does not change between power cycles
uint32 gyro_device_id # Gyroscope unique device ID for the sensor that does not change between power cycles
float32[3] delta_angle # delta angle in the NED board axis in rad/s over the integration time frame (dt)
float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt)
uint16 dt # integration period in us
uint8 integrated_samples # number of samples integrated
uint8 clip_count # total clip count per integration period on any axis

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

@ -71,6 +71,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
@ -92,7 +93,7 @@
using math::constrain;
using namespace time_literals;
class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams, public px4::WorkItem
class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
explicit Ekf2(bool replay_mode = false);
@ -129,7 +130,7 @@ private:
template<typename Param>
bool update_mag_decl(Param &mag_decl_param);
bool publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now);
bool publish_attitude(const hrt_abstime &now);
bool publish_wind_estimate(const hrt_abstime &timestamp);
/*
@ -241,7 +242,15 @@ private:
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
static constexpr int MAX_SENSOR_COUNT = 3;
uORB::SubscriptionCallbackWorkItem _vehicle_imu_subs[MAX_SENSOR_COUNT] {
{this, ORB_ID(vehicle_imu), 0},
{this, ORB_ID(vehicle_imu), 1},
{this, ORB_ID(vehicle_imu), 2}
};
int _imu_sub_index{-1};
bool _callback_registered{false};
// because we can have several distance sensor instances with different orientations
uORB::Subscription _range_finder_subs[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}};
@ -418,6 +427,8 @@ private:
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
(ParamInt<px4::params::EKF2_IMU_ID>) _param_ekf2_imu_id,
// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
@ -521,7 +532,7 @@ private:
Ekf2::Ekf2(bool replay_mode):
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_replay_mode(replay_mode),
_ekf_update_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": update")),
_params(_ekf.getParamHandle()),
@ -638,14 +649,39 @@ Ekf2::~Ekf2()
perf_free(_ekf_update_perf);
}
bool
Ekf2::init()
bool Ekf2::init()
{
if (!_sensors_sub.registerCallback()) {
PX4_ERR("sensor combined callback registration failed!");
return false;
const uint32_t device_id = _param_ekf2_imu_id.get();
// if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined)
if (device_id != 0) {
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
vehicle_imu_s imu{};
if (_vehicle_imu_subs[i].copy(&imu)) {
if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) {
if (_vehicle_imu_subs[i].registerCallback()) {
PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id);
_imu_sub_index = i;
_callback_registered = true;
return true;
}
}
}
}
} else {
_imu_sub_index = -1;
if (_sensor_combined_sub.registerCallback()) {
_callback_registered = true;
return true;
}
}
PX4_WARN("failed to register callback, retrying in 1 second");
ScheduleDelayed(1_s); // retry in 1 second
return true;
}
@ -698,14 +734,56 @@ bool Ekf2::update_mag_decl(Param &mag_decl_param)
void Ekf2::Run()
{
if (should_exit()) {
_sensors_sub.unregisterCallback();
_sensor_combined_sub.unregisterCallback();
for (auto &i : _vehicle_imu_subs) {
i.unregisterCallback();
}
exit_and_cleanup();
return;
}
sensor_combined_s sensors;
if (!_callback_registered) {
init();
return;
}
if (_sensors_sub.update(&sensors)) {
bool updated = false;
imuSample imu_sample_new;
hrt_abstime imu_dt = 0; // for tracking time slip later
sensor_bias_s bias{};
if (_imu_sub_index >= 0) {
vehicle_imu_s imu;
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 = Vector3f{imu.delta_angle};
imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
imu_dt = imu.dt;
bias.accel_device_id = imu.accel_device_id;
bias.gyro_device_id = imu.gyro_device_id;
} else {
sensor_combined_s sensor_combined;
updated = _sensor_combined_sub.update(&sensor_combined);
imu_sample_new.time_us = sensor_combined.timestamp;
imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f;
imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt;
imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
imu_dt = sensor_combined.gyro_integral_dt;
}
if (updated) {
// check for parameter updates
if (_parameter_update_sub.updated()) {
@ -717,9 +795,11 @@ void Ekf2::Run()
updateParams();
}
const hrt_abstime now = imu_sample_new.time_us;
// ekf2_timestamps (using 0.1 ms relative timestamps)
ekf2_timestamps_s ekf2_timestamps{};
ekf2_timestamps.timestamp = sensors.timestamp;
ekf2_timestamps.timestamp = now;
ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
@ -747,14 +827,17 @@ void Ekf2::Run()
if (_sensor_selection_sub.copy(&_sensor_selection)) {
if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
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;
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;
}
}
}
}
@ -765,20 +848,11 @@ void Ekf2::Run()
_imu_bias_reset_request = !_ekf.reset_imu_bias();
}
const hrt_abstime now = sensors.timestamp;
// push imu data into estimator
imuSample imu_sample_new;
imu_sample_new.time_us = now;
imu_sample_new.delta_ang_dt = sensors.gyro_integral_dt * 1.e-6f;
imu_sample_new.delta_ang = Vector3f{sensors.gyro_rad} * imu_sample_new.delta_ang_dt;
imu_sample_new.delta_vel_dt = sensors.accelerometer_integral_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{sensors.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
_ekf.setIMUData(imu_sample_new);
// publish attitude immediately (uses quaternion from output predictor)
publish_attitude(sensors, now);
publish_attitude(now);
// read mag data
if (_magnetometer_sub.updated()) {
@ -1106,7 +1180,7 @@ void Ekf2::Run()
// run the EKF update and output
perf_begin(_ekf_update_perf);
const bool updated = _ekf.update();
const bool ekf_updated = _ekf.update();
perf_end(_ekf_update_perf);
// integrate time to monitor time slippage
@ -1115,11 +1189,11 @@ void Ekf2::Run()
_last_time_slip_us = 0;
} else if (_start_time_us > 0) {
_integrated_time_us += sensors.gyro_integral_dt;
_integrated_time_us += imu_dt;
_last_time_slip_us = (now - _start_time_us) - _integrated_time_us;
}
if (updated) {
if (ekf_updated) {
filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value);
@ -1205,9 +1279,10 @@ void Ekf2::Run()
// Vehicle odometry angular rates
float gyro_bias[3];
_ekf.get_gyro_bias(gyro_bias);
odom.rollspeed = sensors.gyro_rad[0] - gyro_bias[0];
odom.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1];
odom.yawspeed = sensors.gyro_rad[2] - gyro_bias[2];
const Vector3f rates{imu_sample_new.delta_ang * imu_sample_new.delta_ang_dt};
odom.rollspeed = rates(0) - gyro_bias[0];
odom.pitchspeed = rates(1) - gyro_bias[1];
odom.yawspeed = rates(2) - gyro_bias[2];
lpos.dist_bottom_valid = _ekf.get_terrain_valid();
@ -1382,12 +1457,14 @@ void Ekf2::Run()
{
// publish all corrected sensor readings and bias estimates after mag calibration is updated above
sensor_bias_s bias{};
bias.timestamp = now;
bias.gyro_device_id = _sensor_selection.gyro_device_id;
bias.accel_device_id = _sensor_selection.accel_device_id;
// take device ids from sensor_selection_s if not using specific vehicle_imu_s
if (_imu_sub_index < 0) {
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
@ -1593,7 +1670,7 @@ void Ekf2::Run()
// calculate noise filtered velocity innovations which are used for pre-flight checking
if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
float dt_seconds = sensors.accelerometer_integral_dt * 1e-6f;
float dt_seconds = imu_sample_new.delta_ang_dt;
runPreFlightChecks(dt_seconds, control_status, _vehicle_status, innovations);
} else {
@ -1671,7 +1748,7 @@ int Ekf2::getRangeSubIndex()
return -1;
}
bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now)
bool Ekf2::publish_attitude(const hrt_abstime &now)
{
if (_ekf.attitude_valid()) {
// generate vehicle attitude quaternion data

13
src/modules/ekf2/ekf2_params.c
View File

@ -805,6 +805,19 @@ PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f);
*/
PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f);
/**
* Device id of IMU
*
* Set to 0 to use system selected (sensor_combined) IMU,
* otherwise set to the device id of the desired IMU (vehicle_imu).
*
* @group EKF2
* @value 0 System Primary
* @category Developer
*
*/
PARAM_DEFINE_INT32(EKF2_IMU_ID, 0);
/**
* X position of IMU in body frame
*

2
src/modules/sensors/CMakeLists.txt
View File

@ -33,6 +33,7 @@
add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_imu)
px4_add_module(
MODULE modules__sensors
@ -51,4 +52,5 @@ px4_add_module(
mathlib
vehicle_acceleration
vehicle_angular_velocity
vehicle_imu
)

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

@ -260,7 +260,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
/**
* Gyro control data maximum publication rate
*
* This is the maximum rate the gyro control data (sensor_gyro_control) will be allowed to publish at.
* This is the maximum rate the gyro control data (sensor_gyro) will be allowed to publish at.
* Set to 0 to disable and publish at the native sensor sample rate.
*
* @min 0

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

@ -72,6 +72,7 @@
#include "voted_sensors_update.h"
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_imu/VehicleIMU.hpp"
using namespace sensors;
using namespace time_literals;
@ -141,6 +142,9 @@ private:
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
/**
* Update our local parameter cache.
@ -173,6 +177,8 @@ private:
*/
void adc_poll();
void InitializeVehicleIMU();
};
Sensors::Sensors(bool hil_enabled) :
@ -188,12 +194,20 @@ Sensors::Sensors(bool hil_enabled) :
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
InitializeVehicleIMU();
}
Sensors::~Sensors()
{
_vehicle_acceleration.Stop();
_vehicle_angular_velocity.Stop();
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
i->Stop();
}
}
}
int
@ -376,6 +390,37 @@ Sensors::adc_poll()
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
}
void Sensors::InitializeVehicleIMU()
{
// create a VehicleIMU instance for each accel/gyro pair
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{};
accel_sub.copy(&accel);
uORB::Subscription gyro_sub{ORB_ID(sensor_gyro_integrated), i};
sensor_gyro_integrated_s gyro{};
gyro_sub.copy(&gyro);
if (accel.device_id > 0 && gyro.device_id > 0) {
VehicleIMU *imu = new VehicleIMU(i, i);
if (imu != nullptr) {
// Start VehicleIMU instance and store
if (imu->Start()) {
_vehicle_imu_list[i] = imu;
} else {
delete imu;
}
}
}
}
}
}
void
Sensors::run()
{
@ -482,6 +527,7 @@ Sensors::run()
*/
if (!_armed && hrt_elapsed_time(&last_config_update) > 500_ms) {
_voted_sensors_update.initializeSensors();
InitializeVehicleIMU();
last_config_update = hrt_absolute_time();
} else {
@ -524,6 +570,12 @@ int Sensors::print_status()
_vehicle_acceleration.PrintStatus();
_vehicle_angular_velocity.PrintStatus();
for (auto &i : _vehicle_imu_list) {
if (i != nullptr) {
i->PrintStatus();
}
}
return 0;
}

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

@ -58,14 +58,14 @@ public:
VehicleAcceleration();
~VehicleAcceleration() override;
void Run() override;
bool Start();
void Stop();
bool Start();
void Stop();
void PrintStatus();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
@ -81,26 +81,26 @@ private:
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(sensor_accel), 0},
{this, ORB_ID(sensor_accel), 1},
{this, ORB_ID(sensor_accel), 2}
};
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Dcmf _board_rotation;
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};
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_sub_index{0};
int8_t _corrections_selected_instance{-1};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
};

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

@ -58,14 +58,14 @@ public:
VehicleAngularVelocity();
~VehicleAngularVelocity() override;
void Run() override;
bool Start();
void Stop();
bool Start();
void Stop();
void PrintStatus();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
@ -81,26 +81,26 @@ private:
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(sensor_gyro), 0},
{this, ORB_ID(sensor_gyro), 1},
{this, ORB_ID(sensor_gyro), 2}
};
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Dcmf _board_rotation;
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};
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_sub_index{0};
int8_t _corrections_selected_instance{-1};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
};

38
src/modules/sensors/vehicle_imu/CMakeLists.txt Normal file
View File

@ -0,0 +1,38 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
px4_add_library(vehicle_imu
VehicleIMU.cpp
VehicleIMU.hpp
)
target_link_libraries(vehicle_imu PRIVATE px4_work_queue)

228
src/modules/sensors/vehicle_imu/VehicleIMU.cpp Normal file
View File

@ -0,0 +1,228 @@
/****************************************************************************
*
* Copyright (c) 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 "VehicleIMU.hpp"
#include <px4_platform_common/log.h>
using namespace matrix;
using namespace time_literals;
using math::radians;
VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index),
_sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index)
{
}
VehicleIMU::~VehicleIMU()
{
Stop();
delete _name;
}
bool VehicleIMU::Start()
{
// force initial updates
ParametersUpdate(true);
return _sensor_accel_integrated_sub.registerCallback() && _sensor_gyro_integrated_sub.registerCallback();
}
void VehicleIMU::Stop()
{
Deinit();
// clear all registered callbacks
_sensor_accel_integrated_sub.unregisterCallback();
_sensor_gyro_integrated_sub.unregisterCallback();
}
void VehicleIMU::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections{};
if (_sensor_correction_sub.copy(&corrections)) {
// accel
if (_accel_device_id > 0) {
if ((_corrections_selected_accel_instance < 0) || force) {
_corrections_selected_accel_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.accel_device_ids[i] == _accel_device_id) {
_corrections_selected_accel_instance = i;
}
}
}
switch (_corrections_selected_accel_instance) {
case 0:
_accel_offset = Vector3f{corrections.accel_offset_0};
_accel_scale = Vector3f{corrections.accel_scale_0};
break;
case 1:
_accel_offset = Vector3f{corrections.accel_offset_1};
_accel_scale = Vector3f{corrections.accel_scale_1};
break;
case 2:
_accel_offset = Vector3f{corrections.accel_offset_2};
_accel_scale = Vector3f{corrections.accel_scale_2};
break;
default:
_accel_offset = Vector3f{0.f, 0.f, 0.f};
_accel_scale = Vector3f{1.f, 1.f, 1.f};
}
}
// gyro
if (_gyro_device_id > 0) {
if ((_corrections_selected_gyro_instance < 0) || force) {
_corrections_selected_gyro_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.gyro_device_ids[i] == _gyro_device_id) {
_corrections_selected_gyro_instance = i;
}
}
}
switch (_corrections_selected_gyro_instance) {
case 0:
_gyro_offset = Vector3f{corrections.gyro_offset_0};
_gyro_scale = Vector3f{corrections.gyro_scale_0};
break;
case 1:
_gyro_offset = Vector3f{corrections.gyro_offset_1};
_gyro_scale = Vector3f{corrections.gyro_scale_1};
break;
case 2:
_gyro_offset = Vector3f{corrections.gyro_offset_2};
_gyro_scale = Vector3f{corrections.gyro_scale_2};
break;
default:
_gyro_offset = Vector3f{0.f, 0.f, 0.f};
_gyro_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
}
}
void VehicleIMU::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
// get transformation matrix from sensor/board to body frame
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
}
}
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_device_id = accel.device_id;
sensor_gyro_integrated_s gyro;
_sensor_gyro_integrated_sub.copy(&gyro);
_gyro_device_id = gyro.device_id;
SensorCorrectionsUpdate();
ParametersUpdate();
// delta angle: apply offsets, scale, and board rotation
Vector3f delta_angle{gyro.delta_angle};
const float gyro_dt = 1.e-6f * gyro.dt;
// apply offsets
delta_angle = delta_angle - (_gyro_offset * gyro_dt);
// apply scale
delta_angle = delta_angle.emult(_gyro_scale);
// apply board rotation
delta_angle = _board_rotation * delta_angle;
// delta velocity: apply offsets, scale, and board rotation
Vector3f delta_velocity{accel.delta_velocity};
const float accel_dt = 1.e-6f * accel.dt;
// apply offsets
delta_velocity = delta_velocity - (_accel_offset * accel_dt);
// apply scale
delta_velocity = delta_velocity.emult(_accel_scale);
// apply board rotation
delta_velocity = _board_rotation * delta_velocity;
// 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.integrated_samples = accel.samples;
imu.clip_count = accel.clip_count;
imu.timestamp = hrt_absolute_time();
_vehicle_imu_pub.publish(imu);
}
}
void VehicleIMU::PrintStatus()
{
PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_device_id, _gyro_device_id);
}

104
src/modules/sensors/vehicle_imu/VehicleIMU.hpp Normal file
View File

@ -0,0 +1,104 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
#pragma once
#include <containers/List.hpp>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/WorkItem.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_correction.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/vehicle_imu.h>
class VehicleIMU : public ModuleParams, public px4::WorkItem
{
public:
VehicleIMU() = delete;
VehicleIMU(uint8_t accel_index = 0, uint8_t gyro_index = 0);
~VehicleIMU() override;
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ParametersUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
uORB::SubscriptionCallbackWorkItem _sensor_accel_integrated_sub;
uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub;
matrix::Dcmf _board_rotation;
matrix::Vector3f _accel_offset{0.f, 0.f, 0.f};
matrix::Vector3f _gyro_offset{0.f, 0.f, 0.f};
matrix::Vector3f _accel_scale{1.f, 1.f, 1.f};
matrix::Vector3f _gyro_scale{1.f, 1.f, 1.f};
char *_name{nullptr};
int8_t _corrections_selected_accel_instance{-1};
int8_t _corrections_selected_gyro_instance{-1};
uint32_t _accel_device_id{0};
uint32_t _gyro_device_id{0};
};