diff --git a/.vscode/settings.json b/.vscode/settings.json index 35c3146dcf..67016f0983 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -127,5 +127,6 @@ "terminal.integrated.scrollback": 15000, "yaml.schemas": { "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml" - } + }, + "ros.distro": "humble" } diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index db358c3e4d..5a01b3754d 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +CONFIG_EKF2_MOCAP=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 46faddafac..5473148cc7 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -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_airspeed estimator_aid_src_sideslip # 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 diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 9fdba8fefa..2adf84731d 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -19,4 +19,4 @@ bool innovation_rejected # true if the observation has been rejected 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_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 diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 0f1c095890..d4e8833649 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -99,6 +99,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_MOCAP) + list(APPEND EKF_SRCS EKF/mocap.cpp) +endif() + if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 64230774af..84b19af192 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -64,6 +64,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_MOCAP) + list(APPEND EKF_SRCS mocap.cpp) +endif() + if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS auxvel_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index c35563135c..ce761f410a 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -111,6 +111,10 @@ void Ekf::controlFusionModes(const imuSample &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) controlAirDataFusion(imu_delayed); #endif // CONFIG_EKF2_AIRSPEED diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 7010626d89..20cbae9fc2 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -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. _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 +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a42ea5bcee..19efe49bcf 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -54,6 +54,10 @@ #include #include +#if defined(CONFIG_EKF2_MOCAP) +# include "mocap.hpp" +#endif // CONFIG_EKF2_MOCAP + enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface @@ -266,6 +270,8 @@ public: // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; + auto& getState() const { return _state; } + // get the wind velocity var 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; } #endif // CONFIG_EKF2_AUXVEL + void updateParameters(); + + friend class Mocap; + private: // 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 status.innovation_rejected = innovation_rejected; } + +#if defined(CONFIG_EKF2_MOCAP) && defined(MODULE_NAME) + Mocap _mocap{}; +#endif // CONFIG_EKF2_MOCAP }; #endif // !EKF_EKF_H diff --git a/src/modules/ekf2/EKF/mocap.cpp b/src/modules/ekf2/EKF/mocap.cpp new file mode 100644 index 0000000000..91da9add59 --- /dev/null +++ b/src/modules/ekf2/EKF/mocap.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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(_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)) { + + if (!ekf.global_origin_valid()) { + return; + } + + // position + { + 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 + + // TODO: check frame + if (ekf.control_status_flags().yaw_align) { + 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 + } + + } +} + +#endif // CONFIG_EKF2_MOCAP diff --git a/src/modules/ekf2/EKF/mocap.hpp b/src/modules/ekf2/EKF/mocap.hpp new file mode 100644 index 0000000000..cc0473736f --- /dev/null +++ b/src/modules/ekf2/EKF/mocap.hpp @@ -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 +# include +# include +# include +# include +# include +#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 _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_src_mocap_xy_pub {ORB_ID(estimator_aid_src_mocap_xy)}; + uORB::PublicationMulti _estimator_aid_src_mocap_z_pub{ORB_ID(estimator_aid_src_mocap_z)}; + uORB::PublicationMulti _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) _param_ekf2_mocap_ctrl, + (ParamFloat) _param_ekf2_mocap_delay, + (ParamFloat) _param_ekf2_mocap_noise, + (ParamFloat) _param_ekf2_mocap_gate + ) + +#endif // MODULE_NAME +}; + +#endif // CONFIG_EKF2_MOCAP + +#endif // !EKF_MOCAP_HPP diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index ef33773998..788a01ce1f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -450,6 +450,8 @@ void EKF2::Run() } } } + + _ekf.updateParameters(); } if (!_callback_registered) { diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 6d27130363..e4dc7dda25 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -21,6 +21,13 @@ depends on MODULES_EKF2 ---help--- 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 depends on MODULES_EKF2 bool "aux velocity fusion support" diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 76fd6192a9..d576ba1ab0 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1568,3 +1568,53 @@ PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0); * @decimal 1 */ PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f); + +/** + * Motion capture (mocap) aiding + * + * Set bits in the following positions to enable: + * 0 : Horizontal position fusion + * 1 : Vertical position fusion + * 2 : Yaw + * + * @group EKF2 + * @min 0 + * @max 15 + * @bit 0 Horizontal position + * @bit 1 Vertical position + * @bit 2 Yaw + */ +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); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3e729489a9..11616c3241 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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 * 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_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_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_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);