mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-26 07:23:34 +08:00
ekf2: split GNSS velocity and position fusion
This commit is contained in:
Daniel Agar
@@ -174,8 +174,10 @@ endif()
|
|||||||
if(CONFIG_EKF2_GNSS)
|
if(CONFIG_EKF2_GNSS)
|
||||||
list(APPEND EKF_SRCS
|
list(APPEND EKF_SRCS
|
||||||
EKF/aid_sources/gnss/gnss_height_control.cpp
|
EKF/aid_sources/gnss/gnss_height_control.cpp
|
||||||
EKF/aid_sources/gnss/gps_checks.cpp
|
EKF/aid_sources/gnss/gnss_checks.cpp
|
||||||
EKF/aid_sources/gnss/gps_control.cpp
|
EKF/aid_sources/gnss/gnss_control.cpp
|
||||||
|
EKF/aid_sources/gnss/gnss_position.cpp
|
||||||
|
EKF/aid_sources/gnss/gnss_velocity.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
if(CONFIG_EKF2_GNSS_YAW)
|
if(CONFIG_EKF2_GNSS_YAW)
|
||||||
|
|||||||
@@ -91,8 +91,10 @@ endif()
|
|||||||
if(CONFIG_EKF2_GNSS)
|
if(CONFIG_EKF2_GNSS)
|
||||||
list(APPEND EKF_SRCS
|
list(APPEND EKF_SRCS
|
||||||
aid_sources/gnss/gnss_height_control.cpp
|
aid_sources/gnss/gnss_height_control.cpp
|
||||||
aid_sources/gnss/gps_checks.cpp
|
aid_sources/gnss/gnss_checks.cpp
|
||||||
aid_sources/gnss/gps_control.cpp
|
aid_sources/gnss/gnss_control.cpp
|
||||||
|
aid_sources/gnss/gnss_position.cpp
|
||||||
|
aid_sources/gnss/gnss_velocity.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
if(CONFIG_EKF2_GNSS_YAW)
|
if(CONFIG_EKF2_GNSS_YAW)
|
||||||
|
|||||||
+12
@@ -167,6 +167,18 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
|
|||||||
_last_gps_fail_us = _time_delayed_us;
|
_last_gps_fail_us = _time_delayed_us;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (
|
||||||
|
_gps_check_fail_status.flags.fix ||
|
||||||
|
(_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
|
||||||
|
(_gps_check_fail_status.flags.pdop && (_params.gps_check_mask & MASK_GPS_PDOP)) ||
|
||||||
|
(_gps_check_fail_status.flags.spoofed && (_params.gps_check_mask & MASK_GPS_SPOOFED))
|
||||||
|
) {
|
||||||
|
_gnss_common_checks_passed = false;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_gnss_common_checks_passed = true;
|
||||||
|
}
|
||||||
|
|
||||||
// if any user selected checks have failed, record the fail time
|
// if any user selected checks have failed, record the fail time
|
||||||
if (
|
if (
|
||||||
_gps_check_fail_status.flags.fix ||
|
_gps_check_fail_status.flags.fix ||
|
||||||
@@ -0,0 +1,163 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2021-2025 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 <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
void Ekf::controlGnssFusion(const imuSample &imu_sample)
|
||||||
|
{
|
||||||
|
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
|
||||||
|
stopGnssFusion();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!gyro_bias_inhibited()) {
|
||||||
|
_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);
|
||||||
|
}
|
||||||
|
|
||||||
|
// run EKF-GSF yaw estimator once per IMU update
|
||||||
|
_yawEstimator.predict(imu_sample.delta_ang, imu_sample.delta_ang_dt,
|
||||||
|
imu_sample.delta_vel, imu_sample.delta_vel_dt,
|
||||||
|
(_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest));
|
||||||
|
|
||||||
|
|
||||||
|
// check for arrival of new sensor data at the fusion time horizon
|
||||||
|
gnssSample gnss_sample;
|
||||||
|
|
||||||
|
if (_gps_buffer->pop_first_older_than(imu_sample.time_us, &gnss_sample)) {
|
||||||
|
|
||||||
|
if (runGnssChecks(gnss_sample)
|
||||||
|
&& isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
|
||||||
|
if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
|
||||||
|
// First time checks are passing, latching.
|
||||||
|
if (!_gps_checks_passed) {
|
||||||
|
_information_events.flags.gps_checks_passed = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
_gps_checks_passed = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// Skip this sample
|
||||||
|
_gps_data_ready = false;
|
||||||
|
|
||||||
|
if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)
|
||||||
|
&& isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
|
||||||
|
stopGnssFusion();
|
||||||
|
ECL_WARN("GNSS quality poor - stopping use");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
controlGnssHeightFusion(gnss_sample);
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||||
|
controlGnssYawFusion(gnss_sample);
|
||||||
|
#endif // CONFIG_EKF2_GNSS_YAW
|
||||||
|
|
||||||
|
controlGnssYawEstimator(_aid_src_gnss_vel);
|
||||||
|
|
||||||
|
bool do_vel_pos_reset = false;
|
||||||
|
|
||||||
|
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||||
|
// Reset checks need to run together, before each control function runs because the result would change
|
||||||
|
// after the first one runs
|
||||||
|
do_vel_pos_reset = shouldResetGpsFusion();
|
||||||
|
|
||||||
|
if (_control_status.flags.in_air
|
||||||
|
&& isYawFailure()
|
||||||
|
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
||||||
|
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
|
||||||
|
do_vel_pos_reset = tryYawEmergencyReset();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
controlGnssVelocityFusion(imu_sample, gnss_sample, do_vel_pos_reset);
|
||||||
|
controlGnssPositionFusion(imu_sample, gnss_sample, do_vel_pos_reset);
|
||||||
|
|
||||||
|
|
||||||
|
} else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||||
|
if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
|
||||||
|
stopGnssFusion();
|
||||||
|
ECL_WARN("GNSS data stopped");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Ekf::shouldResetGpsFusion() const
|
||||||
|
{
|
||||||
|
/* We are relying on aiding to constrain drift so after a specified time
|
||||||
|
* with no aiding we need to do something
|
||||||
|
*/
|
||||||
|
bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||||
|
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
|
|
||||||
|
if (has_horizontal_aiding_timed_out) {
|
||||||
|
// horizontal aiding hasn't timed out if optical flow still active
|
||||||
|
if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
|
||||||
|
has_horizontal_aiding_timed_out = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
|
|
||||||
|
const bool is_reset_required = has_horizontal_aiding_timed_out
|
||||||
|
|| (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max)
|
||||||
|
&& (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
|
||||||
|
|
||||||
|
const bool is_inflight_nav_failure = _control_status.flags.in_air
|
||||||
|
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||||
|
&& isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||||
|
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)
|
||||||
|
&& (_time_last_hor_pos_fuse > _time_last_on_ground_us);
|
||||||
|
|
||||||
|
return (is_reset_required || is_inflight_nav_failure);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf::stopGnssFusion()
|
||||||
|
{
|
||||||
|
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||||
|
_last_gps_fail_us = 0;
|
||||||
|
_last_gps_pass_us = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
stopGnssVelFusion();
|
||||||
|
stopGnssPosFusion();
|
||||||
|
stopGpsHgtFusion();
|
||||||
|
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||||
|
stopGnssYawFusion();
|
||||||
|
#endif // CONFIG_EKF2_GNSS_YAW
|
||||||
|
|
||||||
|
_yawEstimator.reset();
|
||||||
|
}
|
||||||
@@ -0,0 +1,142 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2021-2025 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 <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
void Ekf::controlGnssPositionFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, bool reset)
|
||||||
|
{
|
||||||
|
static constexpr const char *AID_SRC_NAME = "GNSS position";
|
||||||
|
|
||||||
|
auto &aid_src = _aid_src_gnss_pos;
|
||||||
|
|
||||||
|
// correct position and height for offset relative to IMU
|
||||||
|
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||||
|
const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
|
||||||
|
const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
|
||||||
|
const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);
|
||||||
|
const Vector2f innovation = (_gpos - measurement_corrected).xy();
|
||||||
|
|
||||||
|
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
|
||||||
|
float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
|
||||||
|
|
||||||
|
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
|
||||||
|
// if we are not using another source of aiding, then we are reliant on the GNSS
|
||||||
|
// observations to constrain attitude errors and must limit the observation noise value.
|
||||||
|
if (pos_noise > _params.pos_noaid_noise) {
|
||||||
|
pos_noise = _params.pos_noaid_noise;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const float pos_var = math::max(sq(pos_noise), sq(0.01f));
|
||||||
|
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||||
|
const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg());
|
||||||
|
|
||||||
|
updateAidSourceStatus(aid_src,
|
||||||
|
gnss_sample.time_us, // sample timestamp
|
||||||
|
observation, // observation
|
||||||
|
pos_obs_var, // observation variance
|
||||||
|
innovation, // innovation
|
||||||
|
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||||
|
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
||||||
|
|
||||||
|
const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
|
||||||
|
|
||||||
|
const bool continuing_conditions_passing = gnss_pos_enabled
|
||||||
|
&& _control_status.flags.tilt_align
|
||||||
|
&& _control_status.flags.yaw_align
|
||||||
|
&& (gnss_sample.hacc < _params.req_hacc);
|
||||||
|
|
||||||
|
const bool starting_conditions_passing = continuing_conditions_passing
|
||||||
|
&& _gnss_common_checks_passed;
|
||||||
|
|
||||||
|
const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
|
||||||
|
|
||||||
|
if (_control_status.flags.gnss_pos) {
|
||||||
|
if (continuing_conditions_passing) {
|
||||||
|
if (reset) {
|
||||||
|
ECL_WARN("%s fusion timeout, resetting", AID_SRC_NAME);
|
||||||
|
|
||||||
|
_information_events.flags.reset_pos_to_gps = true;
|
||||||
|
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||||
|
aid_src.observation_variance[0] + aid_src.observation_variance[1]);
|
||||||
|
|
||||||
|
resetAidSourceStatusZeroInnovation(aid_src);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
fuseHorizontalPosition(aid_src);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
stopGnssPosFusion();
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (starting_conditions_passing) {
|
||||||
|
ECL_INFO("%s starting fusion", AID_SRC_NAME);
|
||||||
|
_information_events.flags.starting_gps_fusion = true;
|
||||||
|
|
||||||
|
_information_events.flags.reset_pos_to_gps = true;
|
||||||
|
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||||
|
aid_src.observation_variance[0] +
|
||||||
|
aid_src.observation_variance[1]);
|
||||||
|
|
||||||
|
resetAidSourceStatusZeroInnovation(aid_src);
|
||||||
|
|
||||||
|
_control_status.flags.gnss_pos = true;
|
||||||
|
|
||||||
|
} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
|
||||||
|
|
||||||
|
_information_events.flags.reset_pos_to_gps = true;
|
||||||
|
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||||
|
aid_src.observation_variance[0] +
|
||||||
|
aid_src.observation_variance[1]);
|
||||||
|
|
||||||
|
resetAidSourceStatusZeroInnovation(aid_src);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf::stopGnssPosFusion()
|
||||||
|
{
|
||||||
|
if (_control_status.flags.gnss_pos) {
|
||||||
|
ECL_INFO("stopping GNSS position fusion");
|
||||||
|
_control_status.flags.gnss_pos = false;
|
||||||
|
|
||||||
|
//TODO: what if gnss yaw or height is used?
|
||||||
|
if (!_control_status.flags.gnss_vel) {
|
||||||
|
_last_gps_fail_us = 0;
|
||||||
|
_last_gps_pass_us = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,283 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2021-2025 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 <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
void Ekf::controlGnssVelocityFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, bool reset)
|
||||||
|
{
|
||||||
|
static constexpr const char *AID_SRC_NAME = "GNSS velocity";
|
||||||
|
|
||||||
|
auto &aid_src = _aid_src_gnss_vel;
|
||||||
|
|
||||||
|
// correct velocity for offset relative to IMU
|
||||||
|
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||||
|
|
||||||
|
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
|
||||||
|
const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
|
||||||
|
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||||
|
const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
|
||||||
|
|
||||||
|
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
|
||||||
|
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
|
||||||
|
|
||||||
|
const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
|
||||||
|
|
||||||
|
updateAidSourceStatus(aid_src,
|
||||||
|
gnss_sample.time_us, // sample timestamp
|
||||||
|
velocity, // observation
|
||||||
|
vel_obs_var, // observation variance
|
||||||
|
_state.vel - velocity, // innovation
|
||||||
|
getVelocityVariance() + vel_obs_var, // innovation variance
|
||||||
|
innovation_gate); // innovation gate
|
||||||
|
|
||||||
|
// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
|
||||||
|
// but limit innovation to prevent spikes that could destabilise the filter
|
||||||
|
bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical
|
||||||
|
&& (aid_src.test_ratio[2] > 1.f) // vz rejected
|
||||||
|
&& (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted
|
||||||
|
|
||||||
|
if (bad_acc_vz_rejected
|
||||||
|
&& (gnss_sample.sacc < _params.req_sacc)
|
||||||
|
) {
|
||||||
|
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
|
||||||
|
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
|
||||||
|
aid_src.innovation_rejected = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_control_status.flags.gnss_vel) {
|
||||||
|
|
||||||
|
if (_control_status.flags.in_air
|
||||||
|
&& isYawFailure()
|
||||||
|
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
||||||
|
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
|
||||||
|
|
||||||
|
if (tryYawEmergencyReset()) {
|
||||||
|
reset = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
||||||
|
&& _control_status.flags.tilt_align
|
||||||
|
&& _control_status.flags.yaw_align
|
||||||
|
&& velocity.isAllFinite()
|
||||||
|
&& (gnss_sample.sacc < _params.req_sacc)
|
||||||
|
&& !_gps_check_fail_status.flags.hspeed;
|
||||||
|
|
||||||
|
const bool starting_conditions_passing = continuing_conditions_passing
|
||||||
|
&& _gnss_common_checks_passed
|
||||||
|
&& velocity.longerThan(0.f);
|
||||||
|
|
||||||
|
if (_control_status.flags.gnss_vel) {
|
||||||
|
if (continuing_conditions_passing) {
|
||||||
|
if (reset) {
|
||||||
|
ECL_WARN("%s fusion timeout, resetting", AID_SRC_NAME);
|
||||||
|
|
||||||
|
_information_events.flags.reset_vel_to_gps = true;
|
||||||
|
resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
|
||||||
|
|
||||||
|
resetAidSourceStatusZeroInnovation(aid_src);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
fuseVelocity(aid_src);
|
||||||
|
|
||||||
|
if (isHeightResetRequired()) {
|
||||||
|
// reset vertical velocity if height is failing
|
||||||
|
resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
stopGnssVelFusion();
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (starting_conditions_passing) {
|
||||||
|
ECL_INFO("%s starting fusion", AID_SRC_NAME);
|
||||||
|
_information_events.flags.starting_gps_fusion = true;
|
||||||
|
|
||||||
|
// when already using another velocity source velocity reset is not necessary
|
||||||
|
if (!isHorizontalAidingActive()
|
||||||
|
|| isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||||
|
|| !_control_status_prev.flags.yaw_align
|
||||||
|
) {
|
||||||
|
// reset velocity
|
||||||
|
_information_events.flags.reset_vel_to_gps = true;
|
||||||
|
resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
|
||||||
|
|
||||||
|
resetAidSourceStatusZeroInnovation(aid_src);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
_control_status.flags.gnss_vel = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
|
||||||
|
{
|
||||||
|
// update yaw estimator velocity (basic sanity check on GNSS velocity data)
|
||||||
|
const float vel_var = aid_src_vel.observation_variance[0];
|
||||||
|
const Vector2f vel_xy(aid_src_vel.observation);
|
||||||
|
|
||||||
|
if ((vel_var > 0.f)
|
||||||
|
&& (vel_var < _params.req_sacc)
|
||||||
|
&& vel_xy.isAllFinite()
|
||||||
|
) {
|
||||||
|
_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
|
||||||
|
|
||||||
|
// Try to align yaw using estimate if available
|
||||||
|
if ((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
||||||
|
&& !_control_status.flags.yaw_align
|
||||||
|
&& _control_status.flags.tilt_align
|
||||||
|
) {
|
||||||
|
|
||||||
|
if (resetYawToEKFGSF()) {
|
||||||
|
ECL_INFO("GNSS yaw aligned using IMU");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Ekf::tryYawEmergencyReset()
|
||||||
|
{
|
||||||
|
bool success = false;
|
||||||
|
|
||||||
|
/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
|
||||||
|
* fails while the difference between the yaw emergency estimator and the yaw estimate is large.
|
||||||
|
* This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
|
||||||
|
* present before flight to prevent triggering due to GPS glitches or other sensor errors.
|
||||||
|
*/
|
||||||
|
if (resetYawToEKFGSF()) {
|
||||||
|
ECL_WARN("GNSS emergency yaw reset");
|
||||||
|
|
||||||
|
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||||
|
// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
|
||||||
|
// and cause another navigation failure
|
||||||
|
_control_status.flags.mag_fault = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||||
|
|
||||||
|
if (_control_status.flags.gnss_yaw) {
|
||||||
|
_control_status.flags.gnss_yaw_fault = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // CONFIG_EKF2_GNSS_YAW
|
||||||
|
|
||||||
|
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||||
|
|
||||||
|
if (_control_status.flags.ev_yaw) {
|
||||||
|
_control_status.flags.ev_yaw_fault = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||||
|
|
||||||
|
success = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ekf::stopGnssVelFusion()
|
||||||
|
{
|
||||||
|
if (_control_status.flags.gnss_vel) {
|
||||||
|
ECL_INFO("stopping GNSS velocity fusion");
|
||||||
|
_control_status.flags.gnss_vel = false;
|
||||||
|
|
||||||
|
//TODO: what if gnss yaw or height is used?
|
||||||
|
if (!_control_status.flags.gnss_pos) {
|
||||||
|
_last_gps_fail_us = 0;
|
||||||
|
_last_gps_pass_us = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Ekf::isYawEmergencyEstimateAvailable() const
|
||||||
|
{
|
||||||
|
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
|
||||||
|
// data and the yaw estimate has converged
|
||||||
|
if (!_yawEstimator.isActive()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float yaw_var = _yawEstimator.getYawVar();
|
||||||
|
|
||||||
|
return (yaw_var > 0.f)
|
||||||
|
&& (yaw_var < sq(_params.EKFGSF_yaw_err_max))
|
||||||
|
&& PX4_ISFINITE(yaw_var);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Ekf::isYawFailure() const
|
||||||
|
{
|
||||||
|
if (!isYawEmergencyEstimateAvailable()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float euler_yaw = getEulerYaw(_R_to_earth);
|
||||||
|
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
|
||||||
|
|
||||||
|
return fabsf(yaw_error) > math::radians(25.f);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Ekf::resetYawToEKFGSF()
|
||||||
|
{
|
||||||
|
if (!isYawEmergencyEstimateAvailable()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// don't allow reset if there's just been a yaw reset
|
||||||
|
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
|
||||||
|
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
|
||||||
|
|
||||||
|
if (yaw_alignment_changed || quat_reset) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
|
||||||
|
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
|
||||||
|
|
||||||
|
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
|
||||||
|
|
||||||
|
_control_status.flags.yaw_align = true;
|
||||||
|
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
||||||
|
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
|
||||||
|
{
|
||||||
|
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
|
||||||
|
}
|
||||||
@@ -67,8 +67,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
|
|||||||
|
|
||||||
const bool starting_conditions_passing = continuing_conditions_passing
|
const bool starting_conditions_passing = continuing_conditions_passing
|
||||||
&& _gps_checks_passed
|
&& _gps_checks_passed
|
||||||
&& !is_gnss_yaw_data_intermittent
|
&& !is_gnss_yaw_data_intermittent;
|
||||||
&& !_gps_intermittent;
|
|
||||||
|
|
||||||
if (_control_status.flags.gnss_yaw) {
|
if (_control_status.flags.gnss_yaw) {
|
||||||
if (continuing_conditions_passing) {
|
if (continuing_conditions_passing) {
|
||||||
|
|||||||
@@ -1,495 +0,0 @@
|
|||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (c) 2021-2022 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.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file gps_control.cpp
|
|
||||||
* Control functions for ekf GNSS fusion
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "ekf.h"
|
|
||||||
#include <mathlib/mathlib.h>
|
|
||||||
|
|
||||||
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
|
||||||
{
|
|
||||||
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
|
|
||||||
stopGnssFusion();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!gyro_bias_inhibited()) {
|
|
||||||
_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);
|
|
||||||
}
|
|
||||||
|
|
||||||
// run EKF-GSF yaw estimator once per imu_delayed update
|
|
||||||
_yawEstimator.predict(imu_delayed.delta_ang, imu_delayed.delta_ang_dt,
|
|
||||||
imu_delayed.delta_vel, imu_delayed.delta_vel_dt,
|
|
||||||
(_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest));
|
|
||||||
|
|
||||||
_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
|
|
||||||
|
|
||||||
// check for arrival of new sensor data at the fusion time horizon
|
|
||||||
_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
|
|
||||||
|
|
||||||
if (_gps_data_ready) {
|
|
||||||
const gnssSample &gnss_sample = _gps_sample_delayed;
|
|
||||||
|
|
||||||
if (runGnssChecks(gnss_sample)
|
|
||||||
&& isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
|
|
||||||
if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
|
|
||||||
// First time checks are passing, latching.
|
|
||||||
if (!_gps_checks_passed) {
|
|
||||||
_information_events.flags.gps_checks_passed = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
_gps_checks_passed = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// Skip this sample
|
|
||||||
_gps_data_ready = false;
|
|
||||||
|
|
||||||
if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)
|
|
||||||
&& isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
|
|
||||||
stopGnssFusion();
|
|
||||||
ECL_WARN("GNSS quality poor - stopping use");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
updateGnssPos(gnss_sample, _aid_src_gnss_pos);
|
|
||||||
updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
|
|
||||||
|
|
||||||
} else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
|
||||||
if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
|
|
||||||
stopGnssFusion();
|
|
||||||
ECL_WARN("GNSS data stopped");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_gps_data_ready) {
|
|
||||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
|
||||||
const gnssSample &gnss_sample = _gps_sample_delayed;
|
|
||||||
controlGnssYawFusion(gnss_sample);
|
|
||||||
#endif // CONFIG_EKF2_GNSS_YAW
|
|
||||||
|
|
||||||
controlGnssYawEstimator(_aid_src_gnss_vel);
|
|
||||||
|
|
||||||
bool do_vel_pos_reset = false;
|
|
||||||
|
|
||||||
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
|
||||||
// Reset checks need to run together, before each control function runs because the result would change
|
|
||||||
// after the first one runs
|
|
||||||
do_vel_pos_reset = shouldResetGpsFusion();
|
|
||||||
|
|
||||||
if (_control_status.flags.in_air
|
|
||||||
&& isYawFailure()
|
|
||||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
|
||||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
|
|
||||||
do_vel_pos_reset = tryYawEmergencyReset();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
controlGnssVelFusion(_aid_src_gnss_vel, do_vel_pos_reset);
|
|
||||||
controlGnssPosFusion(_aid_src_gnss_pos, do_vel_pos_reset);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset)
|
|
||||||
{
|
|
||||||
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
|
||||||
&& _control_status.flags.tilt_align
|
|
||||||
&& _control_status.flags.yaw_align;
|
|
||||||
const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
|
|
||||||
|
|
||||||
if (_control_status.flags.gnss_vel) {
|
|
||||||
if (continuing_conditions_passing) {
|
|
||||||
if (force_reset) {
|
|
||||||
ECL_WARN("GNSS fusion timeout, resetting");
|
|
||||||
resetVelocityToGnss(aid_src);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
fuseVelocity(aid_src);
|
|
||||||
|
|
||||||
if (isHeightResetRequired()) {
|
|
||||||
// reset vertical velocity if height is failing
|
|
||||||
resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
stopGnssVelFusion();
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
if (starting_conditions_passing) {
|
|
||||||
ECL_INFO("starting GNSS velocity fusion");
|
|
||||||
_information_events.flags.starting_gps_fusion = true;
|
|
||||||
|
|
||||||
// when already using another velocity source velocity reset is not necessary
|
|
||||||
if (!isHorizontalAidingActive()
|
|
||||||
|| isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
|
||||||
|| !_control_status_prev.flags.yaw_align
|
|
||||||
) {
|
|
||||||
// reset velocity
|
|
||||||
resetVelocityToGnss(aid_src);
|
|
||||||
}
|
|
||||||
|
|
||||||
_control_status.flags.gnss_vel = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset)
|
|
||||||
{
|
|
||||||
const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
|
|
||||||
|
|
||||||
const bool continuing_conditions_passing = gnss_pos_enabled
|
|
||||||
&& _control_status.flags.tilt_align
|
|
||||||
&& _control_status.flags.yaw_align;
|
|
||||||
const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
|
|
||||||
const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
|
|
||||||
|
|
||||||
if (_control_status.flags.gnss_pos) {
|
|
||||||
if (continuing_conditions_passing) {
|
|
||||||
if (force_reset) {
|
|
||||||
ECL_WARN("GNSS fusion timeout, resetting");
|
|
||||||
resetHorizontalPositionToGnss(aid_src);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
fuseHorizontalPosition(aid_src);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
stopGnssPosFusion();
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
|
||||||
if (starting_conditions_passing) {
|
|
||||||
ECL_INFO("starting GNSS position fusion");
|
|
||||||
_information_events.flags.starting_gps_fusion = true;
|
|
||||||
|
|
||||||
resetHorizontalPositionToGnss(aid_src);
|
|
||||||
_control_status.flags.gnss_pos = true;
|
|
||||||
|
|
||||||
} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
|
|
||||||
resetHorizontalPositionToGnss(aid_src);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
|
|
||||||
{
|
|
||||||
// correct velocity for offset relative to IMU
|
|
||||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
|
||||||
|
|
||||||
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
|
|
||||||
const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
|
|
||||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
|
||||||
const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
|
|
||||||
|
|
||||||
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
|
|
||||||
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
|
|
||||||
|
|
||||||
const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
|
|
||||||
|
|
||||||
updateAidSourceStatus(aid_src,
|
|
||||||
gnss_sample.time_us, // sample timestamp
|
|
||||||
velocity, // observation
|
|
||||||
vel_obs_var, // observation variance
|
|
||||||
_state.vel - velocity, // innovation
|
|
||||||
getVelocityVariance() + vel_obs_var, // innovation variance
|
|
||||||
innovation_gate); // innovation gate
|
|
||||||
|
|
||||||
// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
|
|
||||||
// but limit innovation to prevent spikes that could destabilise the filter
|
|
||||||
bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical
|
|
||||||
&& (aid_src.test_ratio[2] > 1.f) // vz rejected
|
|
||||||
&& (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted
|
|
||||||
|
|
||||||
if (bad_acc_vz_rejected
|
|
||||||
&& (gnss_sample.sacc < _params.req_sacc)
|
|
||||||
) {
|
|
||||||
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
|
|
||||||
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
|
|
||||||
aid_src.innovation_rejected = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src)
|
|
||||||
{
|
|
||||||
// correct position and height for offset relative to IMU
|
|
||||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
|
||||||
const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
|
|
||||||
const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
|
|
||||||
const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);
|
|
||||||
const Vector2f innovation = (_gpos - measurement_corrected).xy();
|
|
||||||
|
|
||||||
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
|
|
||||||
float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
|
|
||||||
|
|
||||||
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
|
|
||||||
// if we are not using another source of aiding, then we are reliant on the GNSS
|
|
||||||
// observations to constrain attitude errors and must limit the observation noise value.
|
|
||||||
if (pos_noise > _params.pos_noaid_noise) {
|
|
||||||
pos_noise = _params.pos_noaid_noise;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const float pos_var = math::max(sq(pos_noise), sq(0.01f));
|
|
||||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
|
||||||
const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg());
|
|
||||||
|
|
||||||
updateAidSourceStatus(aid_src,
|
|
||||||
gnss_sample.time_us, // sample timestamp
|
|
||||||
observation, // observation
|
|
||||||
pos_obs_var, // observation variance
|
|
||||||
innovation, // innovation
|
|
||||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
|
||||||
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
|
|
||||||
{
|
|
||||||
// update yaw estimator velocity (basic sanity check on GNSS velocity data)
|
|
||||||
const float vel_var = aid_src_vel.observation_variance[0];
|
|
||||||
const Vector2f vel_xy(aid_src_vel.observation);
|
|
||||||
|
|
||||||
if ((vel_var > 0.f)
|
|
||||||
&& (vel_var < _params.req_sacc)
|
|
||||||
&& vel_xy.isAllFinite()) {
|
|
||||||
|
|
||||||
_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
|
|
||||||
|
|
||||||
// Try to align yaw using estimate if available
|
|
||||||
if (((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
|
||||||
|| (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
|
|
||||||
&& !_control_status.flags.yaw_align
|
|
||||||
&& _control_status.flags.tilt_align) {
|
|
||||||
if (resetYawToEKFGSF()) {
|
|
||||||
ECL_INFO("GPS yaw aligned using IMU");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Ekf::tryYawEmergencyReset()
|
|
||||||
{
|
|
||||||
bool success = false;
|
|
||||||
|
|
||||||
/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
|
|
||||||
* fails while the difference between the yaw emergency estimator and the yaw estimate is large.
|
|
||||||
* This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
|
|
||||||
* present before flight to prevent triggering due to GPS glitches or other sensor errors.
|
|
||||||
*/
|
|
||||||
if (resetYawToEKFGSF()) {
|
|
||||||
ECL_WARN("GPS emergency yaw reset");
|
|
||||||
|
|
||||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
|
||||||
// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
|
|
||||||
// and cause another navigation failure
|
|
||||||
_control_status.flags.mag_fault = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
|
||||||
|
|
||||||
if (_control_status.flags.gnss_yaw) {
|
|
||||||
_control_status.flags.gnss_yaw_fault = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_GNSS_YAW
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
|
||||||
|
|
||||||
if (_control_status.flags.ev_yaw) {
|
|
||||||
_control_status.flags.ev_yaw_fault = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
|
||||||
|
|
||||||
success = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
return success;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
|
|
||||||
{
|
|
||||||
_information_events.flags.reset_vel_to_gps = true;
|
|
||||||
resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
|
|
||||||
|
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
|
|
||||||
{
|
|
||||||
_information_events.flags.reset_pos_to_gps = true;
|
|
||||||
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
|
||||||
aid_src.observation_variance[0] +
|
|
||||||
aid_src.observation_variance[1]);
|
|
||||||
|
|
||||||
resetAidSourceStatusZeroInnovation(aid_src);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Ekf::shouldResetGpsFusion() const
|
|
||||||
{
|
|
||||||
/* We are relying on aiding to constrain drift so after a specified time
|
|
||||||
* with no aiding we need to do something
|
|
||||||
*/
|
|
||||||
bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
|
||||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
|
||||||
|
|
||||||
if (has_horizontal_aiding_timed_out) {
|
|
||||||
// horizontal aiding hasn't timed out if optical flow still active
|
|
||||||
if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
|
|
||||||
has_horizontal_aiding_timed_out = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
|
||||||
|
|
||||||
const bool is_reset_required = has_horizontal_aiding_timed_out
|
|
||||||
|| (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max)
|
|
||||||
&& (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
|
|
||||||
|
|
||||||
const bool is_inflight_nav_failure = _control_status.flags.in_air
|
|
||||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
|
||||||
&& isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
|
||||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)
|
|
||||||
&& (_time_last_hor_pos_fuse > _time_last_on_ground_us);
|
|
||||||
|
|
||||||
return (is_reset_required || is_inflight_nav_failure);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::stopGnssFusion()
|
|
||||||
{
|
|
||||||
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
|
||||||
_last_gps_fail_us = 0;
|
|
||||||
_last_gps_pass_us = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
stopGnssVelFusion();
|
|
||||||
stopGnssPosFusion();
|
|
||||||
stopGpsHgtFusion();
|
|
||||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
|
||||||
stopGnssYawFusion();
|
|
||||||
#endif // CONFIG_EKF2_GNSS_YAW
|
|
||||||
|
|
||||||
_yawEstimator.reset();
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::stopGnssVelFusion()
|
|
||||||
{
|
|
||||||
if (_control_status.flags.gnss_vel) {
|
|
||||||
ECL_INFO("stopping GNSS velocity fusion");
|
|
||||||
_control_status.flags.gnss_vel = false;
|
|
||||||
|
|
||||||
//TODO: what if gnss yaw or height is used?
|
|
||||||
if (!_control_status.flags.gnss_pos) {
|
|
||||||
_last_gps_fail_us = 0;
|
|
||||||
_last_gps_pass_us = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Ekf::stopGnssPosFusion()
|
|
||||||
{
|
|
||||||
if (_control_status.flags.gnss_pos) {
|
|
||||||
ECL_INFO("stopping GNSS position fusion");
|
|
||||||
_control_status.flags.gnss_pos = false;
|
|
||||||
|
|
||||||
//TODO: what if gnss yaw or height is used?
|
|
||||||
if (!_control_status.flags.gnss_vel) {
|
|
||||||
_last_gps_fail_us = 0;
|
|
||||||
_last_gps_pass_us = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Ekf::isYawEmergencyEstimateAvailable() const
|
|
||||||
{
|
|
||||||
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
|
|
||||||
// data and the yaw estimate has converged
|
|
||||||
if (!_yawEstimator.isActive()) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
const float yaw_var = _yawEstimator.getYawVar();
|
|
||||||
|
|
||||||
return (yaw_var > 0.f)
|
|
||||||
&& (yaw_var < sq(_params.EKFGSF_yaw_err_max))
|
|
||||||
&& PX4_ISFINITE(yaw_var);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Ekf::isYawFailure() const
|
|
||||||
{
|
|
||||||
if (!isYawEmergencyEstimateAvailable()) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
const float euler_yaw = getEulerYaw(_R_to_earth);
|
|
||||||
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
|
|
||||||
|
|
||||||
return fabsf(yaw_error) > math::radians(25.f);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Ekf::resetYawToEKFGSF()
|
|
||||||
{
|
|
||||||
if (!isYawEmergencyEstimateAvailable()) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// don't allow reset if there's just been a yaw reset
|
|
||||||
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
|
|
||||||
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
|
|
||||||
|
|
||||||
if (yaw_alignment_changed || quat_reset) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
|
|
||||||
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
|
|
||||||
|
|
||||||
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
|
|
||||||
|
|
||||||
_control_status.flags.yaw_align = true;
|
|
||||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
|
||||||
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
|
|
||||||
{
|
|
||||||
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
|
|
||||||
}
|
|
||||||
@@ -112,7 +112,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
|||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_GNSS)
|
#if defined(CONFIG_EKF2_GNSS)
|
||||||
controlGpsFusion(imu_delayed);
|
controlGnssFusion(imu_delayed);
|
||||||
#endif // CONFIG_EKF2_GNSS
|
#endif // CONFIG_EKF2_GNSS
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
|
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
|
||||||
|
|||||||
+15
-14
@@ -562,11 +562,11 @@ private:
|
|||||||
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
|
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
|
||||||
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
|
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
|
||||||
uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
|
uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
|
||||||
|
|
||||||
bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed
|
bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed
|
||||||
|
bool _gnss_common_checks_passed{false};
|
||||||
|
|
||||||
gps_check_fail_status_u _gps_check_fail_status{};
|
gps_check_fail_status_u _gps_check_fail_status{};
|
||||||
// height sensor status
|
|
||||||
bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent
|
|
||||||
|
|
||||||
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
|
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
|
||||||
|
|
||||||
@@ -861,19 +861,8 @@ private:
|
|||||||
|
|
||||||
#if defined(CONFIG_EKF2_GNSS)
|
#if defined(CONFIG_EKF2_GNSS)
|
||||||
// control fusion of GPS observations
|
// control fusion of GPS observations
|
||||||
void controlGpsFusion(const imuSample &imu_delayed);
|
void controlGnssFusion(const imuSample &imu_sample);
|
||||||
void controlGnssVelFusion(estimator_aid_source3d_s &aid_src, bool force_reset);
|
|
||||||
void controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset);
|
|
||||||
void stopGnssFusion();
|
void stopGnssFusion();
|
||||||
void stopGnssVelFusion();
|
|
||||||
void stopGnssPosFusion();
|
|
||||||
void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
|
|
||||||
void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
|
|
||||||
void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
|
|
||||||
bool tryYawEmergencyReset();
|
|
||||||
void resetVelocityToGnss(estimator_aid_source3d_s &aid_src);
|
|
||||||
void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src);
|
|
||||||
bool shouldResetGpsFusion() const;
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Return true if the GPS solution quality is adequate.
|
* Return true if the GPS solution quality is adequate.
|
||||||
@@ -882,6 +871,18 @@ private:
|
|||||||
*/
|
*/
|
||||||
bool runGnssChecks(const gnssSample &gps);
|
bool runGnssChecks(const gnssSample &gps);
|
||||||
|
|
||||||
|
|
||||||
|
void controlGnssVelocityFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, const bool force_reset);
|
||||||
|
void stopGnssVelFusion();
|
||||||
|
void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
|
||||||
|
bool tryYawEmergencyReset();
|
||||||
|
|
||||||
|
void controlGnssPositionFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, const bool force_reset);
|
||||||
|
void stopGnssPosFusion();
|
||||||
|
|
||||||
|
bool shouldResetGpsFusion() const;
|
||||||
|
|
||||||
|
|
||||||
void controlGnssHeightFusion(const gnssSample &gps_sample);
|
void controlGnssHeightFusion(const gnssSample &gps_sample);
|
||||||
void stopGpsHgtFusion();
|
void stopGpsHgtFusion();
|
||||||
|
|
||||||
|
|||||||
@@ -87,8 +87,6 @@ public:
|
|||||||
#if defined(CONFIG_EKF2_GNSS)
|
#if defined(CONFIG_EKF2_GNSS)
|
||||||
void setGpsData(const gnssSample &gnss_sample);
|
void setGpsData(const gnssSample &gnss_sample);
|
||||||
|
|
||||||
const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
|
|
||||||
|
|
||||||
float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
|
float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
|
||||||
float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
|
float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
|
||||||
float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
|
float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
|
||||||
@@ -395,8 +393,6 @@ protected:
|
|||||||
RingBuffer<gnssSample> *_gps_buffer {nullptr};
|
RingBuffer<gnssSample> *_gps_buffer {nullptr};
|
||||||
uint64_t _time_last_gps_buffer_push{0};
|
uint64_t _time_last_gps_buffer_push{0};
|
||||||
|
|
||||||
gnssSample _gps_sample_delayed{};
|
|
||||||
|
|
||||||
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
|
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
|
||||||
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
|
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
|
||||||
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
|
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
|
||||||
|
|||||||
@@ -47,10 +47,6 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
|
|||||||
controlBaroHeightFusion(imu_delayed);
|
controlBaroHeightFusion(imu_delayed);
|
||||||
#endif // CONFIG_EKF2_BAROMETER
|
#endif // CONFIG_EKF2_BAROMETER
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_GNSS)
|
|
||||||
controlGnssHeightFusion(_gps_sample_delayed);
|
|
||||||
#endif // CONFIG_EKF2_GNSS
|
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||||
controlRangeHaglFusion(imu_delayed);
|
controlRangeHaglFusion(imu_delayed);
|
||||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||||
|
|||||||
@@ -1053,11 +1053,11 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp)
|
|||||||
#if defined(CONFIG_EKF2_GNSS)
|
#if defined(CONFIG_EKF2_GNSS)
|
||||||
void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp)
|
void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp)
|
||||||
{
|
{
|
||||||
if (_ekf.get_gps_sample_delayed().time_us != 0) {
|
if ( _ekf.aid_src_gnss_hgt().timestamp_sample != 0) {
|
||||||
const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus();
|
const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus();
|
||||||
|
|
||||||
if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 0.001f) {
|
if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 0.001f) {
|
||||||
_estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_gps_sample_delayed().time_us, timestamp));
|
_estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_gnss_hgt().timestamp_sample, timestamp));
|
||||||
|
|
||||||
_last_gnss_hgt_bias_published = status.bias;
|
_last_gnss_hgt_bias_published = status.bias;
|
||||||
}
|
}
|
||||||
@@ -1221,7 +1221,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
|||||||
#if defined(CONFIG_EKF2_GNSS)
|
#if defined(CONFIG_EKF2_GNSS)
|
||||||
void EKF2::PublishGpsStatus(const hrt_abstime ×tamp)
|
void EKF2::PublishGpsStatus(const hrt_abstime ×tamp)
|
||||||
{
|
{
|
||||||
const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us;
|
const hrt_abstime timestamp_sample = _ekf.aid_src_gnss_pos().timestamp_sample;
|
||||||
|
|
||||||
if (timestamp_sample == _last_gps_status_published) {
|
if (timestamp_sample == _last_gps_status_published) {
|
||||||
return;
|
return;
|
||||||
|
|||||||
Reference in New Issue
Block a user