Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2025-12-08 03:22:27 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

ekf2: split gnss pos/vel flags

Browse Source 
They can be selected independently in the control parameter, so there is
no reason why they should share the same flag.
This commit is contained in:
bresch
2025-03-06 15:43:38 +01:00
committed by Mathieu Bresciani
parent da3a0656d4
commit 2aaa54037c
20 changed files with 200 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/ecl_ekf/analyse_logdata_ekf.py
View File

@@ -111,13 +111,13 @@ def find_checks_that_apply(
innov_fail_checks.append('yaw')
# Velocity Sensor Checks
if (np.amax(estimator_status_flags['cs_gps']) > 0.5):
if (np.amax(estimator_status_flags['cs_gnss_vel']) > 0.5):
sensor_checks.append('vel')
innov_fail_checks.append('velh')
innov_fail_checks.append('velv')
# Position Sensor Checks
if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gps']) > 0.5)
if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gnss_pos']) > 0.5)
or (np.amax(estimator_status_flags['cs_ev_pos']) > 0.5)):
sensor_checks.append('pos')
innov_fail_checks.append('posh')

2
Tools/ecl_ekf/plotting/pdf_report.py
View File

@@ -185,7 +185,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
# plot control mode summary A
data_plot = ControlModeSummaryPlot(
status_flags_time, estimator_status_flags, [['cs_tilt_align', 'cs_yaw_align'],
['cs_gps', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt',
['cs_gnss_pos', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt',
'cs_rng_hgt', 'cs_ev_hgt'], ['cs_mag_hdg', 'cs_mag_3d', 'cs_mag_dec']],
x_label='time (sec)', y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'],
annotation_text=[['tilt alignment', 'yaw alignment'], ['GPS aiding', 'optical flow aiding',

3
msg/EstimatorStatus.msg
View File

@@ -20,7 +20,7 @@ uint8 GPS_CHECK_FAIL_SPOOFED = 10 # 10 : GPS signal is spoofed
uint64 control_mode_flags # Bitmask to indicate EKF logic state
uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete
uint8 CS_YAW_ALIGN = 1 # 1 - true if the filter yaw alignment is complete
uint8 CS_GPS = 2 # 2 - true if GPS measurements are being fused
uint8 CS_GNSS_POS = 2 # 2 - true if GNSS position measurements are being fused
uint8 CS_OPT_FLOW = 3 # 3 - true if optical flow measurements are being fused
uint8 CS_MAG_HDG = 4 # 4 - true if a simple magnetic yaw heading is being fused
uint8 CS_MAG_3D = 5 # 5 - true if 3-axis magnetometer measurement are being fused
@@ -47,6 +47,7 @@ uint8 CS_SYNTHETIC_MAG_Z = 25 # 25 - true when we are using a synthesized measur
uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest
uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used
uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used
uint8 CS_GNSS_VEL = 44 # 44 - true if GNSS velocity measurements are being fused
uint32 filter_fault_flags # Bitmask to indicate EKF internal faults
# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error

3
msg/EstimatorStatusFlags.msg
View File

@@ -6,7 +6,7 @@ uint64 timestamp_sample # the timestamp of the raw data (micro
uint32 control_status_changes # number of filter control status (cs) changes
bool cs_tilt_align # 0 - true if the filter tilt alignment is complete
bool cs_yaw_align # 1 - true if the filter yaw alignment is complete
bool cs_gps # 2 - true if GPS measurement fusion is intended
bool cs_gnss_pos # 2 - true if GNSS position measurement fusion is intended
bool cs_opt_flow # 3 - true if optical flow measurements fusion is intended
bool cs_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended
bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fusion is intended
@@ -48,6 +48,7 @@ bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terra
bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused
bool cs_constant_pos # 42 - true if the vehicle is at a constant position
bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used
bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

3
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@@ -561,7 +561,8 @@ void AirspeedModule::poll_topics()
_gnss_lpos_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
&& (_vehicle_local_position.timestamp > 0)
&& _vehicle_local_position.v_xy_valid
&& _estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
&& (_estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS)
|| _estimator_status.control_mode_flags & (static_cast<uint64_t>(1) << estimator_status_s::CS_GNSS_VEL));
}
void AirspeedModule::update_wind_estimator_sideslip()

2
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View File

@@ -224,7 +224,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
if (_param_sys_has_gps.get()) {
const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS);
const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
if (ekf_gps_fusion) {

18
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
View File

@@ -70,7 +70,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
pos = ev_sample.pos - pos_offset_earth;
pos_cov = matrix::diag(ev_sample.position_var);
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_pos) {
_ev_pos_b_est.setFusionActive();
} else {
@@ -109,7 +109,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
}
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_pos) {
_ev_pos_b_est.setFusionActive();
} else {
@@ -128,8 +128,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
// increase minimum variance if GNSS is active (position reference)
if (_control_status.flags.gnss_pos) {
for (int i = 0; i < 2; i++) {
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
}
@@ -148,7 +148,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
// bias fusion activated (GPS activated)
// bias fusion activated (GNSS position activated)
if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
if (quality_sufficient) {
// reset the bias estimator
@@ -213,7 +213,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
{
// activate fusion
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_pos) {
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
_ev_pos_b_est.setFusionActive();
@@ -240,7 +240,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
if (quality_sufficient) {
if (!_control_status.flags.gps) {
if (!_control_status.flags.gnss_pos) {
ECL_INFO("reset to %s", EV_AID_SRC_NAME);
_information_events.flags.reset_pos_to_vision = true;
resetHorizontalPositionTo(measurement, measurement_var);
@@ -275,14 +275,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
// Data seems good, attempt a reset
ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
if (_control_status.flags.gps && !pos_xy_fusion_failing) {
if (_control_status.flags.gnss_pos && !pos_xy_fusion_failing) {
// reset EV position bias
_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
} else {
_information_events.flags.reset_pos_to_vision = true;
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_pos) {
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);

4
src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp
View File

@@ -72,8 +72,8 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
&& PX4_ISFINITE(aid_src.observation)
&& PX4_ISFINITE(aid_src.observation_variance);
// if GPS enabled don't allow EV yaw if EV isn't NED
if (_control_status.flags.gps && _control_status.flags.yaw_align
// if GNSS is enabled don't allow EV yaw if EV isn't NED
if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) && _control_status.flags.yaw_align
&& (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
) {
continuing_conditions_passing = false;

3
src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp
View File

@@ -96,11 +96,10 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
} else {
if (starting_conditions_passing) {
// Try to activate GNSS yaw fusion
const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
if (!_control_status.flags.in_air
|| !_control_status.flags.yaw_align
|| not_using_ne_aiding) {
|| !isNorthEastAidingActive()) {
// Reset before starting the fusion
if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) {

205
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@@ -42,7 +42,7 @@
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
{
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
stopGpsFusion();
stopGnssFusion();
return;
}
@@ -78,19 +78,20 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
// Skip this sample
_gps_data_ready = false;
if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
stopGpsFusion();
ECL_WARN("GPS quality poor - stopping use");
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.gps) {
} else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
stopGpsFusion();
ECL_WARN("GPS data stopped");
stopGnssFusion();
ECL_WARN("GNSS data stopped");
}
}
@@ -102,81 +103,105 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
controlGnssYawEstimator(_aid_src_gnss_vel);
const bool gnss_vel_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL));
const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
bool do_vel_pos_reset = false;
const bool continuing_conditions_passing = (gnss_vel_enabled || 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_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.gps) {
if (continuing_conditions_passing) {
if (gnss_vel_enabled) {
fuseVelocity(_aid_src_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)) {
do_vel_pos_reset = tryYawEmergencyReset();
}
}
if (gnss_pos_enabled) {
fuseHorizontalPosition(_aid_src_gnss_pos);
}
controlGnssVelFusion(_aid_src_gnss_vel, do_vel_pos_reset);
controlGnssPosFusion(_aid_src_gnss_pos, do_vel_pos_reset);
}
}
bool do_vel_pos_reset = shouldResetGpsFusion();
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.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();
}
if (do_vel_pos_reset) {
ECL_WARN("GPS fusion timeout, resetting");
}
if (gnss_vel_enabled) {
if (do_vel_pos_reset) {
resetVelocityToGnss(_aid_src_gnss_vel);
} else if (isHeightResetRequired()) {
// reset vertical velocity if height is failing
resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]);
}
}
if (gnss_pos_enabled && do_vel_pos_reset) {
resetHorizontalPositionToGnss(_aid_src_gnss_pos);
}
if (_control_status.flags.gnss_vel) {
if (continuing_conditions_passing) {
if (force_reset) {
ECL_WARN("GNSS fusion timeout, resetting");
resetVelocityToGnss(aid_src);
} else {
stopGpsFusion();
fuseVelocity(aid_src);
if (isHeightResetRequired()) {
// reset vertical velocity if height is failing
resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]);
}
}
} else {
if (starting_conditions_passing) {
ECL_INFO("starting GPS fusion");
_information_events.flags.starting_gps_fusion = true;
stopGnssVelFusion();
}
// 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
if (gnss_vel_enabled) {
resetVelocityToGnss(_aid_src_gnss_vel);
}
}
} else {
if (starting_conditions_passing) {
ECL_INFO("starting GNSS velocity fusion");
_information_events.flags.starting_gps_fusion = true;
if (gnss_pos_enabled) {
resetHorizontalPositionToGnss(_aid_src_gnss_pos);
}
_control_status.flags.gps = true;
} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
resetHorizontalPositionToGnss(_aid_src_gnss_pos);
// 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);
}
}
}
@@ -231,8 +256,8 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
// 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.gps)) {
// if we are not using another source of aiding, then we are reliant on the GPS
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;
@@ -366,17 +391,15 @@ bool Ekf::shouldResetGpsFusion() const
return (is_reset_required || is_inflight_nav_failure);
}
void Ekf::stopGpsFusion()
void Ekf::stopGnssFusion()
{
if (_control_status.flags.gps) {
ECL_INFO("stopping GPS position and velocity fusion");
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
_last_gps_fail_us = 0;
_last_gps_pass_us = 0;
_control_status.flags.gps = false;
}
stopGnssVelFusion();
stopGnssPosFusion();
stopGpsHgtFusion();
#if defined(CONFIG_EKF2_GNSS_YAW)
stopGnssYawFusion();
@@ -385,6 +408,34 @@ void Ekf::stopGpsFusion()
_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

15
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@@ -167,11 +167,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
checkMagHeadingConsistency(mag_sample);
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
{
const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !isNorthEastAidingActive();
const bool common_conditions_passing = _control_status.flags.mag
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
@@ -200,7 +197,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
// then the declination must be fused as an observation to prevent long term heading drift
const bool no_ne_aiding_or_not_moving = !using_ne_aiding || _control_status.flags.vehicle_at_rest;
const bool no_ne_aiding_or_not_moving = !isNorthEastAidingActive() || _control_status.flags.vehicle_at_rest;
_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_not_moving;
if (_control_status.flags.mag) {
@@ -321,10 +318,9 @@ void Ekf::stopMagFusion()
resetMagBiasCov();
if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) {
// reset yaw alignment from mag unless using GNSS aiding
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
// reset yaw alignment from mag unless yaw is observable through North-East aiding
if (!using_ne_aiding) {
if (!isNorthEastAidingActive()) {
_control_status.flags.yaw_align = false;
}
}
@@ -480,9 +476,8 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
// Check if there has been enough change in horizontal velocity to make yaw observable
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
if (using_ne_aiding && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) {
if (isNorthEastAidingActive() && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) {
// yaw angle must be observable to consider consistency
_control_status.flags.mag_heading_consistent = true;
}

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

@@ -566,7 +566,7 @@ union filter_control_status_u {
struct {
uint64_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete
uint64_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete
uint64_t gps : 1; ///< 2 - true if GPS measurement fusion is intended
uint64_t gnss_pos : 1; ///< 2 - true if GNSS position measurement fusion is intended
uint64_t opt_flow : 1; ///< 3 - true if optical flow measurements fusion is intended
uint64_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
uint64_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended
@@ -620,8 +620,8 @@ uint64_t mag_heading_consistent :
uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain
uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused
uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position
uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
} flags;
uint64_t value;
};

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

@@ -862,7 +862,11 @@ private:
#if defined(CONFIG_EKF2_GNSS)
// control fusion of GPS observations
void controlGpsFusion(const imuSample &imu_delayed);
void stopGpsFusion();
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 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);

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

@@ -262,7 +262,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
if (_horizontal_deadreckon_time_exceeded) {
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_pos) {
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
@@ -302,10 +302,14 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_pos) {
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
if (_control_status.flags.gnss_vel) {
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_vel.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -446,7 +450,7 @@ float Ekf::getHorizontalVelocityInnovationTestRatio() const
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_vel) {
for (int i = 0; i < 2; i++) { // only xy
test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i]));
}
@@ -488,7 +492,7 @@ float Ekf::getVerticalVelocityInnovationTestRatio() const
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_vel) {
test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2]));
}
@@ -516,7 +520,7 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_pos) {
for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) {
test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
}
@@ -706,7 +710,7 @@ uint16_t Ekf::get_ekf_soln_status() const
soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
#endif // CONFIG_EKF2_TERRAIN
// 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
// 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GNSS or optical flow)
soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos
|| _control_status.flags.vehicle_at_rest;
@@ -714,13 +718,13 @@ uint16_t Ekf::get_ekf_soln_status() const
soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive();
// 512 ESTIMATOR_PRED_POS_HORIZ_ABS True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gps || _control_status.flags.aux_gpos;
soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gnss_pos || _control_status.flags.aux_gpos;
// 1024 ESTIMATOR_GPS_GLITCH True if the EKF has detected a GPS glitch
// 1024 ESTIMATOR_GPS_GLITCH True if the EKF has detected a GNSS glitch
#if defined(CONFIG_EKF2_GNSS)
const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad);
const bool gnss_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
const bool gnss_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
soln_status.flags.gps_glitch = (gnss_vel_innov_bad || gnss_pos_innov_bad);
#endif // CONFIG_EKF2_GNSS
// 2048 ESTIMATOR_ACCEL_ERROR True if the EKF has detected bad accelerometer data
@@ -799,14 +803,14 @@ void Ekf::updateHorizontalDeadReckoningstatus()
bool aiding_expected_in_air = false;
// velocity aiding active
if ((_control_status.flags.gps || _control_status.flags.ev_vel)
if ((_control_status.flags.gnss_vel || _control_status.flags.ev_vel)
&& isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
) {
inertial_dead_reckoning = false;
}
// position aiding active
if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
if ((_control_status.flags.gnss_pos || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
) {
inertial_dead_reckoning = false;

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

@@ -617,7 +617,7 @@ bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool
int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const
{
return int(_control_status.flags.gps)
return int(_control_status.flags.gnss_pos)
+ int(_control_status.flags.ev_pos)
+ int(_control_status.flags.aux_gpos);
}
@@ -672,10 +672,17 @@ bool EstimatorInterface::isVerticalVelocityAidingActive() const
int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
{
return int(_control_status.flags.gps)
return int(_control_status.flags.gnss_vel)
+ int(_control_status.flags.ev_vel);
}
bool EstimatorInterface::isNorthEastAidingActive() const
{
return _control_status.flags.gnss_pos
|| _control_status.flags.gnss_vel
|| _control_status.flags.aux_gpos;
}
void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
{
if (buffer_name) {

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

@@ -227,6 +227,7 @@ public:
// the flags considered are opt_flow, gps, ev_vel and ev_pos
bool isHorizontalAidingActive() const;
bool isVerticalAidingActive() const;
bool isNorthEastAidingActive() const;
int getNumberOfActiveHorizontalAidingSources() const;
int getNumberOfActiveHorizontalPositionAidingSources() const;

2
src/modules/ekf2/EKF/height_control.cpp
View File

@@ -205,7 +205,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance};
}
if (_control_status.flags.gps) {
if (_control_status.flags.gnss_vel) {
checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]};
}

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

@@ -533,7 +533,8 @@ void EKF2::Run()
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning
|| (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gps)) && PX4_ISFINITE(vehicle_command.param2)
|| (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gnss_pos))
&& PX4_ISFINITE(vehicle_command.param2)
&& PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)
) {
@@ -1892,7 +1893,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.control_status_changes = _filter_control_status_changes;
status_flags.cs_tilt_align = _ekf.control_status_flags().tilt_align;
status_flags.cs_yaw_align = _ekf.control_status_flags().yaw_align;
status_flags.cs_gps = _ekf.control_status_flags().gps;
status_flags.cs_gnss_pos = _ekf.control_status_flags().gnss_pos;
status_flags.cs_opt_flow = _ekf.control_status_flags().opt_flow;
status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg;
status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D;
@@ -1934,6 +1935,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos;
status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos;
status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault;
status_flags.cs_gnss_vel = _ekf.control_status_flags().gnss_vel;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;

2
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
View File

@@ -117,7 +117,7 @@ void EkfWrapper::disableGpsFusion()
bool EkfWrapper::isIntendingGpsFusion() const
{
return _ekf->control_status_flags().gps;
return _ekf->control_status_flags().gnss_vel || _ekf->control_status_flags().gnss_pos;
}
void EkfWrapper::enableGpsHeadingFusion()

6
src/modules/ekf2/test/test_EKF_basics.cpp
View File

@@ -103,7 +103,8 @@ TEST_F(EkfBasicsTest, initialControlMode)
// THEN: EKF control status should be reasonable
EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align);
EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gps);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_pos);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_vel);
EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow);
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
@@ -158,7 +159,8 @@ TEST_F(EkfBasicsTest, gpsFusion)
// THEN: EKF should fuse GPS, but no other position sensor
EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align);
EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
EXPECT_EQ(1, (int) _ekf->control_status_flags().gps);
EXPECT_EQ(1, (int) _ekf->control_status_flags().gnss_pos);
EXPECT_EQ(1, (int) _ekf->control_status_flags().gnss_vel);
EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow);
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);