Skip to content

Commit 2aaa540

Browse files
breschbresch
committed
ekf2: split gnss pos/vel flags
They can be selected independently in the control parameter, so there is no reason why they should share the same flag.
1 parent da3a065 commit 2aaa540

20 files changed

+195
-127
lines changed

‎Tools/ecl_ekf/analyse_logdata_ekf.py

+2-2
Original file line numberDiff line numberDiff line change
@@ -111,13 +111,13 @@ def find_checks_that_apply(
111111
innov_fail_checks.append('yaw')
112112

113113
# Velocity Sensor Checks
114-
if (np.amax(estimator_status_flags['cs_gps']) > 0.5):
114+
if (np.amax(estimator_status_flags['cs_gnss_vel']) > 0.5):
115115
sensor_checks.append('vel')
116116
innov_fail_checks.append('velh')
117117
innov_fail_checks.append('velv')
118118

119119
# Position Sensor Checks
120-
if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gps']) > 0.5)
120+
if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gnss_pos']) > 0.5)
121121
or (np.amax(estimator_status_flags['cs_ev_pos']) > 0.5)):
122122
sensor_checks.append('pos')
123123
innov_fail_checks.append('posh')

‎Tools/ecl_ekf/plotting/pdf_report.py

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

‎msg/EstimatorStatus.msg

+2-1
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ uint8 GPS_CHECK_FAIL_SPOOFED = 10 # 10 : GPS signal is spoofed
2020
uint64 control_mode_flags # Bitmask to indicate EKF logic state
2121
uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete
2222
uint8 CS_YAW_ALIGN = 1 # 1 - true if the filter yaw alignment is complete
23-
uint8 CS_GPS = 2 # 2 - true if GPS measurements are being fused
23+
uint8 CS_GNSS_POS = 2 # 2 - true if GNSS position measurements are being fused
2424
uint8 CS_OPT_FLOW = 3 # 3 - true if optical flow measurements are being fused
2525
uint8 CS_MAG_HDG = 4 # 4 - true if a simple magnetic yaw heading is being fused
2626
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
4747
uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest
4848
uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used
4949
uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used
50+
uint8 CS_GNSS_VEL = 44 # 44 - true if GNSS velocity measurements are being fused
5051

5152
uint32 filter_fault_flags # Bitmask to indicate EKF internal faults
5253
# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error

‎msg/EstimatorStatusFlags.msg

+2-1
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@ uint64 timestamp_sample # the timestamp of the raw data (micro
66
uint32 control_status_changes # number of filter control status (cs) changes
77
bool cs_tilt_align # 0 - true if the filter tilt alignment is complete
88
bool cs_yaw_align # 1 - true if the filter yaw alignment is complete
9-
bool cs_gps # 2 - true if GPS measurement fusion is intended
9+
bool cs_gnss_pos # 2 - true if GNSS position measurement fusion is intended
1010
bool cs_opt_flow # 3 - true if optical flow measurements fusion is intended
1111
bool cs_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended
1212
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
4848
bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused
4949
bool cs_constant_pos # 42 - true if the vehicle is at a constant position
5050
bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used
51+
bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended
5152

5253
# fault status
5354
uint32 fault_status_changes # number of filter fault status (fs) changes

‎src/modules/airspeed_selector/airspeed_selector_main.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -561,7 +561,8 @@ void AirspeedModule::poll_topics()
561561
_gnss_lpos_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
562562
&& (_vehicle_local_position.timestamp > 0)
563563
&& _vehicle_local_position.v_xy_valid
564-
&& _estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
564+
&& (_estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS)
565+
|| _estimator_status.control_mode_flags & (static_cast<uint64_t>(1) << estimator_status_s::CS_GNSS_VEL));
565566
}
566567

567568
void AirspeedModule::update_wind_estimator_sideslip()

‎src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -224,7 +224,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
224224

225225
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
226226
if (_param_sys_has_gps.get()) {
227-
const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
227+
const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS);
228228
const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
229229

230230
if (ekf_gps_fusion) {

‎src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

+9-9
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
7070
pos = ev_sample.pos - pos_offset_earth;
7171
pos_cov = matrix::diag(ev_sample.position_var);
7272

73-
if (_control_status.flags.gps) {
73+
if (_control_status.flags.gnss_pos) {
7474
_ev_pos_b_est.setFusionActive();
7575

7676
} else {
@@ -109,7 +109,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
109109
pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max);
110110
}
111111

112-
if (_control_status.flags.gps) {
112+
if (_control_status.flags.gnss_pos) {
113113
_ev_pos_b_est.setFusionActive();
114114

115115
} else {
@@ -128,8 +128,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
128128

129129
#if defined(CONFIG_EKF2_GNSS)
130130

131-
// increase minimum variance if GPS active (position reference)
132-
if (_control_status.flags.gps) {
131+
// increase minimum variance if GNSS is active (position reference)
132+
if (_control_status.flags.gnss_pos) {
133133
for (int i = 0; i < 2; i++) {
134134
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
135135
}
@@ -148,7 +148,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
148148

149149
const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
150150

151-
// bias fusion activated (GPS activated)
151+
// bias fusion activated (GNSS position activated)
152152
if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
153153
if (quality_sufficient) {
154154
// reset the bias estimator
@@ -213,7 +213,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
213213
{
214214
// activate fusion
215215
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
216-
if (_control_status.flags.gps) {
216+
if (_control_status.flags.gnss_pos) {
217217
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
218218
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
219219
_ev_pos_b_est.setFusionActive();
@@ -240,7 +240,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
240240

241241
if (quality_sufficient) {
242242

243-
if (!_control_status.flags.gps) {
243+
if (!_control_status.flags.gnss_pos) {
244244
ECL_INFO("reset to %s", EV_AID_SRC_NAME);
245245
_information_events.flags.reset_pos_to_vision = true;
246246
resetHorizontalPositionTo(measurement, measurement_var);
@@ -275,14 +275,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
275275
// Data seems good, attempt a reset
276276
ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME);
277277

278-
if (_control_status.flags.gps && !pos_xy_fusion_failing) {
278+
if (_control_status.flags.gnss_pos && !pos_xy_fusion_failing) {
279279
// reset EV position bias
280280
_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
281281

282282
} else {
283283
_information_events.flags.reset_pos_to_vision = true;
284284

285-
if (_control_status.flags.gps) {
285+
if (_control_status.flags.gnss_pos) {
286286
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
287287
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
288288

‎src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -72,8 +72,8 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
7272
&& PX4_ISFINITE(aid_src.observation)
7373
&& PX4_ISFINITE(aid_src.observation_variance);
7474

75-
// if GPS enabled don't allow EV yaw if EV isn't NED
76-
if (_control_status.flags.gps && _control_status.flags.yaw_align
75+
// if GNSS is enabled don't allow EV yaw if EV isn't NED
76+
if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) && _control_status.flags.yaw_align
7777
&& (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
7878
) {
7979
continuing_conditions_passing = false;

‎src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -96,11 +96,10 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
9696
} else {
9797
if (starting_conditions_passing) {
9898
// Try to activate GNSS yaw fusion
99-
const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
10099

101100
if (!_control_status.flags.in_air
102101
|| !_control_status.flags.yaw_align
103-
|| not_using_ne_aiding) {
102+
|| !isNorthEastAidingActive()) {
104103

105104
// Reset before starting the fusion
106105
if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) {

0 commit comments

Comments
 (0)