yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00: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
Download Patch File Download Diff File Expand all files Collapse all files

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