forked from Archive/PX4-Autopilot
Compare commits
2 Commits
main
...
pr-ekf2_mo
Author | SHA1 | Date |
---|---|---|
Daniel Agar | 9a86f520e8 | |
Daniel Agar | 0c1535f3e4 |
|
@ -127,5 +127,6 @@
|
||||||
"terminal.integrated.scrollback": 15000,
|
"terminal.integrated.scrollback": 15000,
|
||||||
"yaml.schemas": {
|
"yaml.schemas": {
|
||||||
"${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
|
"${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
|
||||||
}
|
},
|
||||||
|
"ros.distro": "humble"
|
||||||
}
|
}
|
||||||
|
|
|
@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
|
CONFIG_EKF2_MOCAP=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||||
|
|
|
@ -21,4 +21,5 @@ bool fused # true if the sample was successfully fused
|
||||||
# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
|
# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
|
||||||
# TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
|
# TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
|
||||||
# TOPICS estimator_aid_src_fake_hgt
|
# TOPICS estimator_aid_src_fake_hgt
|
||||||
|
# TOPICS estimator_aid_src_mocap_z estimator_aid_src_mocap_yaw
|
||||||
# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
|
# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
|
||||||
|
|
|
@ -19,4 +19,4 @@ bool innovation_rejected # true if the observation has been rejected
|
||||||
bool fused # true if the sample was successfully fused
|
bool fused # true if the sample was successfully fused
|
||||||
|
|
||||||
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
|
# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
|
||||||
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
|
# TOPICS estimator_aid_src_aux_vel estimator_aid_src_mocap_xy estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
|
||||||
|
|
|
@ -42,6 +42,7 @@ bool cs_gravity_vector # 34 - true when gravity vector measurements are
|
||||||
bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
|
bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
|
||||||
bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used
|
bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used
|
||||||
bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter
|
bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter
|
||||||
|
bool cs_mocap # 38 - true when motion capture (mocap) fusion is intended
|
||||||
|
|
||||||
# fault status
|
# fault status
|
||||||
uint32 fault_status_changes # number of filter fault status (fs) changes
|
uint32 fault_status_changes # number of filter fault status (fs) changes
|
||||||
|
|
|
@ -99,6 +99,10 @@ if(CONFIG_EKF2_AIRSPEED)
|
||||||
list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
|
list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
if(CONFIG_EKF2_MOCAP)
|
||||||
|
list(APPEND EKF_SRCS EKF/mocap.cpp)
|
||||||
|
endif()
|
||||||
|
|
||||||
if(CONFIG_EKF2_AUXVEL)
|
if(CONFIG_EKF2_AUXVEL)
|
||||||
list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
|
list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
|
||||||
endif()
|
endif()
|
||||||
|
|
|
@ -64,6 +64,10 @@ if(CONFIG_EKF2_AIRSPEED)
|
||||||
list(APPEND EKF_SRCS airspeed_fusion.cpp)
|
list(APPEND EKF_SRCS airspeed_fusion.cpp)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
if(CONFIG_EKF2_MOCAP)
|
||||||
|
list(APPEND EKF_SRCS mocap.cpp)
|
||||||
|
endif()
|
||||||
|
|
||||||
if(CONFIG_EKF2_AUXVEL)
|
if(CONFIG_EKF2_AUXVEL)
|
||||||
list(APPEND EKF_SRCS auxvel_fusion.cpp)
|
list(APPEND EKF_SRCS auxvel_fusion.cpp)
|
||||||
endif()
|
endif()
|
||||||
|
|
|
@ -113,11 +113,12 @@ enum TerrainFusionMask : uint8_t {
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|
||||||
enum HeightSensor : uint8_t {
|
enum HeightSensor : uint8_t {
|
||||||
BARO = 0,
|
BARO = 0,
|
||||||
GNSS = 1,
|
GNSS = 1,
|
||||||
RANGE = 2,
|
RANGE = 2,
|
||||||
EV = 3,
|
EV = 3,
|
||||||
UNKNOWN = 4
|
MOCAP = 4,
|
||||||
|
UNKNOWN = 5
|
||||||
};
|
};
|
||||||
|
|
||||||
enum class PositionSensor : uint8_t {
|
enum class PositionSensor : uint8_t {
|
||||||
|
@ -601,7 +602,7 @@ union filter_control_status_u {
|
||||||
uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
|
uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
|
||||||
uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
|
uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
|
||||||
uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
|
uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
|
||||||
|
uint64_t mocap : 1; ///< 38 - true when motion capture (mocap) fusion is intended
|
||||||
} flags;
|
} flags;
|
||||||
uint64_t value;
|
uint64_t value;
|
||||||
};
|
};
|
||||||
|
|
|
@ -111,6 +111,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
||||||
|
|
||||||
controlGpsFusion(imu_delayed);
|
controlGpsFusion(imu_delayed);
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
|
||||||
|
_mocap.update(*this, imu_delayed);
|
||||||
|
#endif // CONFIG_EKF2_MOCAP
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||||
controlAirDataFusion(imu_delayed);
|
controlAirDataFusion(imu_delayed);
|
||||||
#endif // CONFIG_EKF2_AIRSPEED
|
#endif // CONFIG_EKF2_AIRSPEED
|
||||||
|
|
|
@ -316,3 +316,10 @@ void Ekf::predictState(const imuSample &imu_delayed)
|
||||||
// Note fixed coefficients are used to save operations. The exact time constant is not important.
|
// Note fixed coefficients are used to save operations. The exact time constant is not important.
|
||||||
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
|
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Ekf::updateParameters()
|
||||||
|
{
|
||||||
|
#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
|
||||||
|
_mocap.updateParameters();
|
||||||
|
#endif // CONFIG_EKF2_MOCAP
|
||||||
|
}
|
||||||
|
|
|
@ -54,6 +54,10 @@
|
||||||
#include <uORB/topics/estimator_aid_source2d.h>
|
#include <uORB/topics/estimator_aid_source2d.h>
|
||||||
#include <uORB/topics/estimator_aid_source3d.h>
|
#include <uORB/topics/estimator_aid_source3d.h>
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_MOCAP)
|
||||||
|
# include "mocap.hpp"
|
||||||
|
#endif // CONFIG_EKF2_MOCAP
|
||||||
|
|
||||||
enum class Likelihood { LOW, MEDIUM, HIGH };
|
enum class Likelihood { LOW, MEDIUM, HIGH };
|
||||||
|
|
||||||
class Ekf final : public EstimatorInterface
|
class Ekf final : public EstimatorInterface
|
||||||
|
@ -266,6 +270,8 @@ public:
|
||||||
// get the wind velocity in m/s
|
// get the wind velocity in m/s
|
||||||
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
|
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
|
||||||
|
|
||||||
|
auto& getState() const { return _state; }
|
||||||
|
|
||||||
// get the wind velocity var
|
// get the wind velocity var
|
||||||
Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
|
Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
|
||||||
|
|
||||||
|
@ -506,6 +512,10 @@ public:
|
||||||
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
|
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
|
||||||
#endif // CONFIG_EKF2_AUXVEL
|
#endif // CONFIG_EKF2_AUXVEL
|
||||||
|
|
||||||
|
void updateParameters();
|
||||||
|
|
||||||
|
friend class Mocap;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
// set the internal states and status to their default value
|
// set the internal states and status to their default value
|
||||||
|
@ -1242,6 +1252,10 @@ private:
|
||||||
// if any of the innovations are rejected, then the overall innovation is rejected
|
// if any of the innovations are rejected, then the overall innovation is rejected
|
||||||
status.innovation_rejected = innovation_rejected;
|
status.innovation_rejected = innovation_rejected;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
|
||||||
|
Mocap _mocap{};
|
||||||
|
#endif // CONFIG_EKF2_MOCAP
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // !EKF_EKF_H
|
#endif // !EKF_EKF_H
|
||||||
|
|
|
@ -270,6 +270,10 @@ public:
|
||||||
const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
|
const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
|
||||||
const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
|
const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
|
||||||
|
|
||||||
|
// TODO
|
||||||
|
void enableControlStatusMocap() { _control_status.flags.mocap = true; }
|
||||||
|
void disableControlStatusMocap() { _control_status.flags.mocap = false; }
|
||||||
|
|
||||||
// get EKF internal fault status
|
// get EKF internal fault status
|
||||||
const fault_status_u &fault_status() const { return _fault_status; }
|
const fault_status_u &fault_status() const { return _fault_status; }
|
||||||
const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }
|
const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }
|
||||||
|
|
|
@ -69,7 +69,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
|
||||||
pos = ev_sample.pos - pos_offset_earth;
|
pos = ev_sample.pos - pos_offset_earth;
|
||||||
pos_cov = matrix::diag(ev_sample.position_var);
|
pos_cov = matrix::diag(ev_sample.position_var);
|
||||||
|
|
||||||
if (_control_status.flags.gps) {
|
if (_control_status.flags.gps || _control_status.flags.mocap) {
|
||||||
_ev_pos_b_est.setFusionActive();
|
_ev_pos_b_est.setFusionActive();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -108,7 +108,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
|
||||||
pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
|
pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_control_status.flags.gps) {
|
if (_control_status.flags.gps || _control_status.flags.mocap) {
|
||||||
_ev_pos_b_est.setFusionActive();
|
_ev_pos_b_est.setFusionActive();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -125,13 +125,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// increase minimum variance if GPS active (position reference)
|
|
||||||
if (_control_status.flags.gps) {
|
|
||||||
for (int i = 0; i < 2; i++) {
|
|
||||||
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const Vector2f measurement{pos(0), pos(1)};
|
const Vector2f measurement{pos(0), pos(1)};
|
||||||
|
|
||||||
const Vector2f measurement_var{
|
const Vector2f measurement_var{
|
||||||
|
@ -200,7 +193,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
|
||||||
{
|
{
|
||||||
// activate fusion
|
// activate fusion
|
||||||
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
|
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
|
||||||
if (_control_status.flags.gps) {
|
if (_control_status.flags.gps || _control_status.flags.mocap) {
|
||||||
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
|
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
|
||||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||||
_ev_pos_b_est.setFusionActive();
|
_ev_pos_b_est.setFusionActive();
|
||||||
|
@ -226,7 +219,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
|
||||||
|
|
||||||
if (quality_sufficient) {
|
if (quality_sufficient) {
|
||||||
|
|
||||||
if (!_control_status.flags.gps) {
|
if (!_control_status.flags.gps && !_control_status.flags.mocap) {
|
||||||
ECL_INFO("reset to %s", EV_AID_SRC_NAME);
|
ECL_INFO("reset to %s", EV_AID_SRC_NAME);
|
||||||
_information_events.flags.reset_pos_to_vision = true;
|
_information_events.flags.reset_pos_to_vision = true;
|
||||||
resetHorizontalPositionTo(measurement, measurement_var);
|
resetHorizontalPositionTo(measurement, measurement_var);
|
||||||
|
@ -261,14 +254,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
|
||||||
// Data seems good, attempt a reset
|
// Data seems good, attempt a reset
|
||||||
ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
|
ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
|
||||||
|
|
||||||
if (_control_status.flags.gps && !pos_xy_fusion_failing) {
|
if ((_control_status.flags.gps || _control_status.flags.mocap) && !pos_xy_fusion_failing) {
|
||||||
// reset EV position bias
|
// reset EV position bias
|
||||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
_information_events.flags.reset_pos_to_vision = true;
|
_information_events.flags.reset_pos_to_vision = true;
|
||||||
|
|
||||||
if (_control_status.flags.gps) {
|
if (_control_status.flags.gps || _control_status.flags.mocap) {
|
||||||
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
|
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
|
||||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||||
|
|
||||||
|
|
|
@ -57,6 +57,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
|
||||||
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
|
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
|
||||||
&& _control_status.flags.tilt_align
|
&& _control_status.flags.tilt_align
|
||||||
&& !_control_status.flags.ev_yaw_fault
|
&& !_control_status.flags.ev_yaw_fault
|
||||||
|
&& !_control_status.flags.mocap
|
||||||
&& PX4_ISFINITE(aid_src.observation)
|
&& PX4_ISFINITE(aid_src.observation)
|
||||||
&& PX4_ISFINITE(aid_src.observation_variance);
|
&& PX4_ISFINITE(aid_src.observation_variance);
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,130 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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 "ekf.h"
|
||||||
|
|
||||||
|
#include "mocap.hpp"
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
|
||||||
|
|
||||||
|
void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
|
||||||
|
{
|
||||||
|
|
||||||
|
#if defined(MODULE_NAME)
|
||||||
|
|
||||||
|
if (_mocap_sub.updated()) {
|
||||||
|
|
||||||
|
vehicle_odometry_s mocap{};
|
||||||
|
_mocap_sub.copy(&mocap);
|
||||||
|
|
||||||
|
const int64_t time_us = mocap.timestamp_sample - static_cast<int64_t>(_param_ekf2_mocap_delay.get() * 1000);
|
||||||
|
|
||||||
|
MocapSample sample{};
|
||||||
|
sample.time_us = time_us;
|
||||||
|
sample.pos = Vector3f{mocap.position};
|
||||||
|
_mocap_buffer.push(sample);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // MODULE_NAME
|
||||||
|
|
||||||
|
|
||||||
|
MocapSample mocap_sample;
|
||||||
|
|
||||||
|
if (_mocap_buffer.pop_first_older_than(imu_delayed.time_us, &mocap_sample)) {
|
||||||
|
|
||||||
|
|
||||||
|
bool pos_valid = mocap_sample.pos.isAllFinite();
|
||||||
|
//bool q_valid = mocap_sample.q.isAllFinite() && (q.min() > 0.f);
|
||||||
|
|
||||||
|
// yaw_align = false;
|
||||||
|
// _height_sensor_ref = HeightSensor::MOCAP;
|
||||||
|
|
||||||
|
//_control_status.flags.mocap = true;
|
||||||
|
|
||||||
|
// reset position and yaw on init
|
||||||
|
|
||||||
|
|
||||||
|
// position xy
|
||||||
|
if (pos_valid) {
|
||||||
|
estimator_aid_source2d_s aid_src{};
|
||||||
|
|
||||||
|
const Vector2f pos_obs_var{
|
||||||
|
math::max(sq(mocap_sample.position_var(0)), sq(_param_ekf2_mocap_noise.get())),
|
||||||
|
math::max(sq(mocap_sample.position_var(1)), sq(_param_ekf2_mocap_noise.get()))
|
||||||
|
};
|
||||||
|
|
||||||
|
ekf.updateHorizontalPositionAidSrcStatus(mocap_sample.time_us,
|
||||||
|
mocap_sample.pos.xy(), // observation
|
||||||
|
pos_obs_var, // observation variance
|
||||||
|
math::max(_param_ekf2_mocap_gate.get(), 1.f), // innovation gate
|
||||||
|
aid_src);
|
||||||
|
aid_src.fusion_enabled = _param_ekf2_mocap_ctrl.get() & 0b001; // bit 0 Horizontal position
|
||||||
|
|
||||||
|
ekf.fuseHorizontalPosition(aid_src);
|
||||||
|
|
||||||
|
#if defined(MODULE_NAME)
|
||||||
|
aid_src.timestamp = hrt_absolute_time();
|
||||||
|
_estimator_aid_src_mocap_xy_pub.publish(aid_src);
|
||||||
|
#endif // MODULE_NAME
|
||||||
|
}
|
||||||
|
|
||||||
|
// position z
|
||||||
|
{
|
||||||
|
estimator_aid_source1d_s aid_src{};
|
||||||
|
|
||||||
|
const float pos_obs_var = math::max(sq(mocap_sample.position_var(2)), sq(_param_ekf2_mocap_noise.get()));
|
||||||
|
|
||||||
|
ekf.updateVerticalPositionAidSrcStatus(mocap_sample.time_us,
|
||||||
|
mocap_sample.pos(2), // observation
|
||||||
|
pos_obs_var, // observation variance
|
||||||
|
math::max(_param_ekf2_mocap_gate.get(), 1.f), // innovation gate
|
||||||
|
aid_src);
|
||||||
|
|
||||||
|
if (_param_ekf2_mocap_ctrl.get()) {
|
||||||
|
aid_src.fusion_enabled = true;
|
||||||
|
|
||||||
|
ekf.fuseVerticalPosition(aid_src);
|
||||||
|
|
||||||
|
//const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if defined(MODULE_NAME)
|
||||||
|
aid_src.timestamp = hrt_absolute_time();
|
||||||
|
_estimator_aid_src_mocap_z_pub.publish(aid_src);
|
||||||
|
#endif // MODULE_NAME
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // CONFIG_EKF2_MOCAP
|
|
@ -0,0 +1,109 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#ifndef EKF_MOCAP_HPP
|
||||||
|
#define EKF_MOCAP_HPP
|
||||||
|
|
||||||
|
#include "common.h"
|
||||||
|
#include "RingBuffer.h"
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME)
|
||||||
|
|
||||||
|
#if defined(MODULE_NAME)
|
||||||
|
# include <px4_platform_common/module_params.h>
|
||||||
|
# include <uORB/PublicationMulti.hpp>
|
||||||
|
# include <uORB/Subscription.hpp>
|
||||||
|
# include <uORB/topics/estimator_aid_source1d.h>
|
||||||
|
# include <uORB/topics/estimator_aid_source2d.h>
|
||||||
|
# include <uORB/topics/vehicle_odometry.h>
|
||||||
|
#endif // MODULE_NAME
|
||||||
|
|
||||||
|
class Ekf;
|
||||||
|
|
||||||
|
class Mocap : public ModuleParams
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Mocap() : ModuleParams(nullptr) {}
|
||||||
|
~Mocap() = default;
|
||||||
|
|
||||||
|
void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
|
||||||
|
|
||||||
|
void updateParameters()
|
||||||
|
{
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
struct MocapSample {
|
||||||
|
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
|
||||||
|
Vector3f pos{}; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
|
||||||
|
Quatf quat{}; ///< quaternion defining rotation from body to earth frame
|
||||||
|
Vector3f position_var{}; ///< XYZ position variances (m**2)
|
||||||
|
Vector3f orientation_var{}; ///< orientation variance (rad**2)
|
||||||
|
PositionFrame pos_frame = PositionFrame::LOCAL_FRAME_FRD;
|
||||||
|
uint8_t reset_counter{};
|
||||||
|
int8_t quality{}; ///< quality indicator between 0 and 100
|
||||||
|
};
|
||||||
|
|
||||||
|
RingBuffer<MocapSample> _mocap_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
|
||||||
|
uint64_t _time_last_buffer_push{0};
|
||||||
|
|
||||||
|
enum state {
|
||||||
|
stopped,
|
||||||
|
starting,
|
||||||
|
active,
|
||||||
|
stopping,
|
||||||
|
resetting,
|
||||||
|
};
|
||||||
|
|
||||||
|
#if defined(MODULE_NAME)
|
||||||
|
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_mocap_xy_pub {ORB_ID(estimator_aid_src_mocap_xy)};
|
||||||
|
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mocap_z_pub{ORB_ID(estimator_aid_src_mocap_z)};
|
||||||
|
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mocap_yaw_pub{ORB_ID(estimator_aid_src_mocap_yaw)};
|
||||||
|
|
||||||
|
uORB::Subscription _mocap_sub{ORB_ID(vehicle_mocap_odometry)};
|
||||||
|
|
||||||
|
DEFINE_PARAMETERS(
|
||||||
|
(ParamInt<px4::params::EKF2_MOCAP_CTRL>) _param_ekf2_mocap_ctrl,
|
||||||
|
(ParamFloat<px4::params::EKF2_MOCAP_DELAY>) _param_ekf2_mocap_delay,
|
||||||
|
(ParamFloat<px4::params::EKF2_MOCAP_NOISE>) _param_ekf2_mocap_noise,
|
||||||
|
(ParamFloat<px4::params::EKF2_MOCAP_GATE>) _param_ekf2_mocap_gate
|
||||||
|
)
|
||||||
|
|
||||||
|
#endif // MODULE_NAME
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // CONFIG_EKF2_MOCAP
|
||||||
|
|
||||||
|
#endif // !EKF_MOCAP_HPP
|
|
@ -450,6 +450,8 @@ void EKF2::Run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_ekf.updateParameters();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_callback_registered) {
|
if (!_callback_registered) {
|
||||||
|
@ -1772,6 +1774,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp)
|
||||||
status_flags.cs_mag = _ekf.control_status_flags().mag;
|
status_flags.cs_mag = _ekf.control_status_flags().mag;
|
||||||
status_flags.cs_ev_yaw_fault = _ekf.control_status_flags().ev_yaw_fault;
|
status_flags.cs_ev_yaw_fault = _ekf.control_status_flags().ev_yaw_fault;
|
||||||
status_flags.cs_mag_heading_consistent = _ekf.control_status_flags().mag_heading_consistent;
|
status_flags.cs_mag_heading_consistent = _ekf.control_status_flags().mag_heading_consistent;
|
||||||
|
status_flags.cs_mocap = _ekf.control_status_flags().mocap;
|
||||||
|
|
||||||
status_flags.fault_status_changes = _filter_fault_status_changes;
|
status_flags.fault_status_changes = _filter_fault_status_changes;
|
||||||
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;
|
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;
|
||||||
|
|
|
@ -21,6 +21,13 @@ depends on MODULES_EKF2
|
||||||
---help---
|
---help---
|
||||||
EKF2 airspeed fusion support.
|
EKF2 airspeed fusion support.
|
||||||
|
|
||||||
|
menuconfig EKF2_MOCAP
|
||||||
|
depends on MODULES_EKF2
|
||||||
|
bool "mocap fusion support"
|
||||||
|
default y
|
||||||
|
---help---
|
||||||
|
EKF2 motion capture (mocap) fusion support.
|
||||||
|
|
||||||
menuconfig EKF2_AUXVEL
|
menuconfig EKF2_AUXVEL
|
||||||
depends on MODULES_EKF2
|
depends on MODULES_EKF2
|
||||||
bool "aux velocity fusion support"
|
bool "aux velocity fusion support"
|
||||||
|
|
|
@ -1568,3 +1568,44 @@ PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0);
|
||||||
* @decimal 1
|
* @decimal 1
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
|
PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Motion capture (mocap) aiding
|
||||||
|
*
|
||||||
|
* @group EKF2
|
||||||
|
* @boolean
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(EKF2_MOCAP_CTRL, 1);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Motion capture (mocap) delay relative to IMU measurements
|
||||||
|
*
|
||||||
|
* @group EKF2
|
||||||
|
* @min 0
|
||||||
|
* @max 300
|
||||||
|
* @unit ms
|
||||||
|
* @reboot_required true
|
||||||
|
* @decimal 1
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(EKF2_MOCAP_DELAY, 0);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Measurement noise for motion capture (mocap) observations used to lower bound or replace the uncertainty included in the message
|
||||||
|
*
|
||||||
|
* @group EKF2
|
||||||
|
* @min 0.01
|
||||||
|
* @unit m
|
||||||
|
* @decimal 2
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(EKF2_MOCAP_NOISE, 0.9f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Gate size for Motion capture (mocap) fusion
|
||||||
|
*
|
||||||
|
* Sets the number of standard deviations used by the innovation consistency test.
|
||||||
|
* @group EKF2
|
||||||
|
* @min 1.0
|
||||||
|
* @unit SD
|
||||||
|
* @decimal 1
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_FLOAT(EKF2_MOCAP_GATE, 3.0f);
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -198,6 +198,7 @@ void LoggedTopics::add_default_topics()
|
||||||
// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
|
// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
|
||||||
// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
|
// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
|
||||||
// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
|
// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
|
||||||
|
add_optional_topic_multi("estimator_aid_src_mocap", 100, MAX_ESTIMATOR_INSTANCES);
|
||||||
// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
|
// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
|
||||||
// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
|
// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
|
||||||
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
|
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
|
||||||
|
|
Loading…
Reference in New Issue