dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: add kconfig option to enable/disable GNSS yaw

This commit is contained in:
Daniel Agar 2023-03-14 10:35:21 -04:00
parent 8b2205810b
commit 98ff1afc19
13 changed files with 142 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/px4/fmu-v2/default.px4board
View File

@ -25,6 +25,7 @@ CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set
# CONFIG_EKF2_GNSS_YAW is not set
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_LAND_DETECTOR=y
CONFIG_MODULES_LOGGER=y

5
src/modules/ekf2/CMakeLists.txt
View File

@ -88,7 +88,6 @@ list(APPEND EKF_SRCS
EKF/gnss_height_control.cpp
EKF/gps_checks.cpp
EKF/gps_control.cpp
EKF/gps_yaw_fusion.cpp
EKF/gravity_fusion.cpp
EKF/height_control.cpp
EKF/imu_down_sampler.cpp
@ -111,6 +110,10 @@ if(CONFIG_EKF2_DRAG_FUSION)
list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
endif()
if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
endif()
px4_add_module(
MODULE modules__ekf2
MAIN ekf2

5
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -53,7 +53,6 @@ list(APPEND EKF_SRCS
gnss_height_control.cpp
gps_checks.cpp
gps_control.cpp
gps_yaw_fusion.cpp
gravity_fusion.cpp
height_control.cpp
imu_down_sampler.cpp
@ -76,6 +75,10 @@ if(CONFIG_EKF2_DRAG_FUSION)
list(APPEND EKF_SRCS drag_fusion.cpp)
endif()
if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
endif()
add_library(ecl_EKF
${EKF_SRCS}
)

2
src/modules/ekf2/EKF/common.h
View File

@ -359,8 +359,10 @@ struct parameters {
float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
float mag_yaw_rate_gate{0.20f}; ///< yaw rate threshold used by mode select logic (rad/sec)
#if defined(CONFIG_EKF2_GNSS_YAW)
// GNSS heading fusion
float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad)
#endif // CONFIG_EKF2_GNSS_YAW
// airspeed fusion
float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD)

3
src/modules/ekf2/EKF/ekf.cpp
View File

@ -130,7 +130,10 @@ void Ekf::reset()
resetEstimatorAidStatus(_aid_src_gnss_hgt);
resetEstimatorAidStatus(_aid_src_gnss_pos);
resetEstimatorAidStatus(_aid_src_gnss_vel);
#if defined(CONFIG_EKF2_GNSS_YAW)
resetEstimatorAidStatus(_aid_src_gnss_yaw);
#endif // CONFIG_EKF2_GNSS_YAW
resetEstimatorAidStatus(_aid_src_mag_heading);
resetEstimatorAidStatus(_aid_src_mag);

70
src/modules/ekf2/EKF/ekf.h
View File

@ -123,15 +123,24 @@ public:
{
if (_control_status.flags.mag_hdg) {
heading_innov = _aid_src_mag_heading.innovation;
return;
}
} else if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_3D) {
heading_innov = Vector3f(_aid_src_mag.innovation).max();
return;
}
} else if (_control_status.flags.gps_yaw) {
#if defined(CONFIG_EKF2_GNSS_YAW)
if (_control_status.flags.gps_yaw) {
heading_innov = _aid_src_gnss_yaw.innovation;
return;
}
#endif // CONFIG_EKF2_GNSS_YAW
} else if (_control_status.flags.ev_yaw) {
if (_control_status.flags.ev_yaw) {
heading_innov = _aid_src_ev_yaw.innovation;
return;
}
}
@ -139,15 +148,24 @@ public:
{
if (_control_status.flags.mag_hdg) {
heading_innov_var = _aid_src_mag_heading.innovation_variance;
return;
}
} else if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_3D) {
heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
return;
}
} else if (_control_status.flags.gps_yaw) {
#if defined(CONFIG_EKF2_GNSS_YAW)
if (_control_status.flags.gps_yaw) {
heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
return;
}
#endif // CONFIG_EKF2_GNSS_YAW
} else if (_control_status.flags.ev_yaw) {
if (_control_status.flags.ev_yaw) {
heading_innov_var = _aid_src_ev_yaw.innovation_variance;
return;
}
}
@ -155,15 +173,24 @@ public:
{
if (_control_status.flags.mag_hdg) {
heading_innov_ratio = _aid_src_mag_heading.test_ratio;
return;
}
} else if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_3D) {
heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
return;
}
} else if (_control_status.flags.gps_yaw) {
#if defined(CONFIG_EKF2_GNSS_YAW)
if (_control_status.flags.gps_yaw) {
heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
return;
}
#endif // CONFIG_EKF2_GNSS_YAW
} else if (_control_status.flags.ev_yaw) {
if (_control_status.flags.ev_yaw) {
heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
return;
}
}
@ -434,7 +461,10 @@ public:
const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
#if defined(CONFIG_EKF2_GNSS_YAW)
const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
#endif // CONFIG_EKF2_GNSS_YAW
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
const auto &aid_src_mag() const { return _aid_src_mag; }
@ -506,7 +536,6 @@ private:
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
uint64_t _time_last_healthy_rng_data{0};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
uint8_t _nb_ev_pos_reset_available{0};
uint8_t _nb_ev_vel_reset_available{0};
@ -574,7 +603,11 @@ private:
estimator_aid_source1d_s _aid_src_gnss_hgt{};
estimator_aid_source2d_s _aid_src_gnss_pos{};
estimator_aid_source3d_s _aid_src_gnss_vel{};
#if defined(CONFIG_EKF2_GNSS_YAW)
estimator_aid_source1d_s _aid_src_gnss_yaw{};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
#endif // CONFIG_EKF2_GNSS_YAW
estimator_aid_source1d_s _aid_src_mag_heading{};
estimator_aid_source3d_s _aid_src_mag{};
@ -668,6 +701,9 @@ private:
bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const;
#if defined(CONFIG_EKF2_GNSS_YAW)
void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw();
@ -675,6 +711,13 @@ private:
// return true if the reset was successful
bool resetYawToGps(const float gnss_yaw);
void updateGpsYaw(const gpsSample &gps_sample);
void startGpsYawFusion(const gpsSample &gps_sample);
#endif // CONFIG_EKF2_GNSS_YAW
void stopGpsYawFusion();
// fuse magnetometer declination measurement
// argument passed in is the declination uncertainty in radians
bool fuseDeclination(float decl_sigma);
@ -741,8 +784,6 @@ private:
void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
void updateGpsYaw(const gpsSample &gps_sample);
// calculate optical flow body angular rate compensation
// returns false if bias corrected body rate data is unavailable
bool calcOptFlowBodyRateComp();
@ -894,8 +935,6 @@ private:
bool shouldResetGpsFusion() const;
bool isYawFailure() const;
void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
// control fusion of magnetometer observations
void controlMagFusion();
@ -1034,9 +1073,6 @@ private:
void stopGpsPosFusion();
void stopGpsVelFusion();
void startGpsYawFusion(const gpsSample &gps_sample);
void stopGpsYawFusion();
void stopEvVelFusion();
void stopEvYawFusion();

23
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -619,19 +619,22 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
status = _innov_check_fail_status.value;
// return the largest magnetometer innovation test ratio
mag = 0.f;
if (_control_status.flags.mag_hdg) {
mag = sqrtf(_aid_src_mag_heading.test_ratio);
} else if (_control_status.flags.mag_3D) {
mag = sqrtf(Vector3f(_aid_src_mag.test_ratio).max());
} else if (_control_status.flags.gps_yaw) {
mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
} else {
mag = NAN;
mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
}
if (_control_status.flags.mag_3D) {
mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
}
#if defined(CONFIG_EKF2_GNSS_YAW)
if (_control_status.flags.gps_yaw) {
mag = math::max(mag, sqrtf(_aid_src_gnss_yaw.test_ratio));
}
#endif // CONFIG_EKF2_GNSS_YAW
// return the largest velocity and position innovation test ratio
vel = NAN;
pos = NAN;

22
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -168,7 +168,17 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
gps_sample_new.hgt = (float)gps.alt * 1e-3f;
gps_sample_new.yaw = gps.yaw;
#if defined(CONFIG_EKF2_GNSS_YAW)
if (PX4_ISFINITE(gps.yaw)) {
_time_last_gps_yaw_buffer_push = _time_latest_us;
gps_sample_new.yaw = gps.yaw;
gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
} else {
gps_sample_new.yaw = NAN;
gps_sample_new.yaw_acc = 0.f;
}
if (PX4_ISFINITE(gps.yaw_offset)) {
_gps_yaw_offset = gps.yaw_offset;
@ -177,12 +187,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
_gps_yaw_offset = 0.0f;
}
if (PX4_ISFINITE(gps.yaw_accuracy)) {
gps_sample_new.yaw_acc = gps.yaw_accuracy;
} else {
gps_sample_new.yaw_acc = 0.f;
}
#endif // CONFIG_EKF2_GNSS_YAW
// Only calculate the relative position if the WGS-84 location of the origin is set
if (collect_gps(gps)) {
@ -196,9 +201,6 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
_gps_buffer->push(gps_sample_new);
_time_last_gps_buffer_push = _time_latest_us;
if (PX4_ISFINITE(gps.yaw)) {
_time_last_gps_yaw_buffer_push = _time_latest_us;
}
} else {
ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);

5
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -312,10 +312,12 @@ protected:
MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
#if defined(CONFIG_EKF2_GNSS_YAW)
float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
// innovation consistency check monitoring ratios
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
uint64_t _time_last_gps_yaw_buffer_push{0};
#endif // CONFIG_EKF2_GNSS_YAW
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
@ -353,7 +355,6 @@ protected:
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
uint64_t _time_last_gps_buffer_push{0};
uint64_t _time_last_gps_yaw_buffer_push{0};
uint64_t _time_last_mag_buffer_push{0};
uint64_t _time_last_baro_buffer_push{0};
uint64_t _time_last_range_buffer_push{0};

50
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -82,7 +82,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
#if defined(CONFIG_EKF2_GNSS_YAW)
controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
#endif // CONFIG_EKF2_GNSS_YAW
// GNSS velocity
const Vector3f velocity{gps_sample.vel};
@ -297,6 +299,7 @@ bool Ekf::isYawFailure() const
return fabsf(yaw_error) > math::radians(25.f);
}
#if defined(CONFIG_EKF2_GNSS_YAW)
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
{
if (!(_params.gnss_ctrl & GnssCtrl::YAW)
@ -387,6 +390,31 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
}
}
void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
{
if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
ECL_INFO("starting GPS yaw fusion");
_control_status.flags.yaw_align = true;
_control_status.flags.mag_dec = false;
stopEvYawFusion();
stopMagHdgFusion();
stopMag3DFusion();
_control_status.flags.gps_yaw = true;
}
}
#endif // CONFIG_EKF2_GNSS_YAW
void Ekf::stopGpsYawFusion()
{
#if defined(CONFIG_EKF2_GNSS_YAW)
if (_control_status.flags.gps_yaw) {
ECL_INFO("stopping GPS yaw fusion");
_control_status.flags.gps_yaw = false;
resetEstimatorAidStatus(_aid_src_gnss_yaw);
}
#endif // CONFIG_EKF2_GNSS_YAW
}
void Ekf::stopGpsFusion()
{
if (_control_status.flags.gps) {
@ -421,25 +449,3 @@ void Ekf::stopGpsVelFusion()
resetEstimatorAidStatus(_aid_src_gnss_vel);
}
void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
{
if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
ECL_INFO("starting GPS yaw fusion");
_control_status.flags.yaw_align = true;
_control_status.flags.mag_dec = false;
stopEvYawFusion();
stopMagHdgFusion();
stopMag3DFusion();
_control_status.flags.gps_yaw = true;
}
}
void Ekf::stopGpsYawFusion()
{
if (_control_status.flags.gps_yaw) {
ECL_INFO("stopping GPS yaw fusion");
_control_status.flags.gps_yaw = false;
resetEstimatorAidStatus(_aid_src_gnss_yaw);
}
}

6
src/modules/ekf2/EKF2.cpp
View File

@ -264,10 +264,14 @@ bool EKF2::multi_init(int imu, int mag)
_estimator_aid_src_gnss_vel_pub.advertise();
}
#if defined(CONFIG_EKF2_GNSS_YAW)
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
_estimator_aid_src_gnss_yaw_pub.advertise();
}
#endif // CONFIG_EKF2_GNSS_YAW
// RNG advertise
if (_param_ekf2_rng_ctrl.get()) {
_estimator_aid_src_rng_hgt_pub.advertise();
@ -823,7 +827,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub);
PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
#if defined(CONFIG_EKF2_GNSS_YAW)
PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
#endif // CONFIG_EKF2_GNSS_YAW
// mag heading
PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);

8
src/modules/ekf2/EKF2.hpp
View File

@ -282,7 +282,9 @@ private:
hrt_abstime _status_gnss_hgt_pub_last{0};
hrt_abstime _status_gnss_pos_pub_last{0};
hrt_abstime _status_gnss_vel_pub_last{0};
hrt_abstime _status_gnss_yaw_pub_last{0};
#if defined(CONFIG_EKF2_GNSS_YAW)
hrt_abstime _status_gnss_yaw_pub_last {0};
#endif // CONFIG_EKF2_GNSS_YAW
hrt_abstime _status_mag_pub_last{0};
hrt_abstime _status_mag_heading_pub_last{0};
@ -375,7 +377,9 @@ private:
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
#if defined(CONFIG_EKF2_GNSS_YAW)
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
#endif // CONFIG_EKF2_GNSS_YAW
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};

7
src/modules/ekf2/Kconfig
View File

@ -27,6 +27,13 @@ depends on MODULES_EKF2
---help---
EKF2 drag fusion support.
menuconfig EKF2_GNSS_YAW
depends on MODULES_EKF2
bool "GNSS yaw fusion support"
default y
---help---
EKF2 GNSS yaw fusion support.
menuconfig USER_EKF2
bool "ekf2 running as userspace module"
default n