From 1b6c5bbafd97a6d2cc82fa304606f6874c050bea Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 May 2016 13:14:52 +1000 Subject: [PATCH] EKF: Enable height source to be selected independent of EV aiding --- EKF/common.h | 1 + EKF/control.cpp | 40 ++++++++++++++++++++++++++++++++-------- EKF/ekf.cpp | 8 ++------ EKF/ekf_helper.cpp | 2 +- EKF/vel_pos_fusion.cpp | 2 +- 5 files changed, 37 insertions(+), 16 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 0dfa16c029..cad93dc713 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -422,6 +422,7 @@ union filter_control_status_u { uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference uint16_t ev_pos : 1; // 12 - true when local position data from external vision is being fused uint16_t ev_yaw : 1; // 13 - true when yaw data from external vision measurements is being fused + uint16_t ev_hgt : 1; // 14 - true when height data from external vision measurements is being fused } flags; uint16_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 0f3afa78f8..ea7565c678 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -323,6 +323,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF baro hgt timeout - reset to GPS\n"); @@ -333,6 +334,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF baro hgt timeout - reset to baro\n"); @@ -372,6 +374,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF gps hgt timeout - reset to baro\n"); @@ -382,6 +385,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF gps hgt timeout - reset to GPS\n"); @@ -414,6 +418,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF rng hgt timeout - reset to baro\n"); @@ -424,6 +429,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; + _control_status.flags.ev_hgt = false; // request a reset reset_height = true; printf("EKF rng hgt timeout - reset to rng hgt\n"); @@ -448,25 +454,43 @@ void Ekf::controlHeightAiding() // check for height sensor timeouts and reset and change sensor if necessary controlHeightSensorTimeouts(); - // Control the soure of height measurements for the main filter - if (_control_status.flags.ev_pos) { - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { + // Control the source of height measurements for the main filter + // do not switch to a sensor if it is unhealthy or the data is stale + if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) && + !_baro_hgt_faulty && + (((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) { + _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; + + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) && + !_gps_hgt_faulty && + (((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) { - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; + + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && + !_rng_hgt_faulty && + (((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) { - } else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; + _control_status.flags.ev_hgt = false; + + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) && + (((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) { + + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = true; + } } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index de6bf563ac..c09126cbb8 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -423,11 +423,7 @@ bool Ekf::initialiseFilter(void) // set the default height source from the adjustable parameter if (_hgt_counter == 0) { - if (_params.fusion_mode & MASK_USE_EVPOS) { - _primary_hgt_source = VDIST_SENSOR_EV; - } else { - _primary_hgt_source = _params.vdist_sensor_type; - } + _primary_hgt_source = _params.vdist_sensor_type; } if (_primary_hgt_source == VDIST_SENSOR_RANGE) { @@ -447,7 +443,7 @@ bool Ekf::initialiseFilter(void) } } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) { - // if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS + // if the user parameter specifies use of GPS or external vision (EV) for height we use baro height initially and switch to GPS or EV // later when it passes checks. if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_hgt_counter == 0 && _baro_sample_delayed.time_us != 0) { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5e7c2720b1..bc33a37692 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -204,7 +204,7 @@ void Ekf::resetHeight() // TODO: reset to last known gps based estimate } - } else if (_control_status.flags.ev_pos) { + } else if (_control_status.flags.ev_hgt) { // initialize vertical position with newest measurement extVisionSample ev_newest = _ext_vision_buffer.get_newest(); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index ab6429f16d..8608e29afe 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -165,7 +165,7 @@ void Ekf::fuseVelPosHeight() R[5] = R[5] * R[5]; // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); - } else if (_control_status.flags.ev_pos) { + } else if (_control_status.flags.ev_hgt) { fuse_map[5] = true; // calculate the innovation assuming the external vision observaton is in local NED frame _vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);