diff --git a/EKF/common.h b/EKF/common.h index f0b40bd611..9ea14ad859 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -145,6 +145,7 @@ struct extVisionSample { #define VDIST_SENSOR_BARO 0 // Use baro height #define VDIST_SENSOR_GPS 1 // Use GPS height #define VDIST_SENSOR_RANGE 2 // Use range finder height +#define VDIST_SENSOR_EV 3 // USe external vision // Bit locations for mag_declination_source #define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value @@ -167,6 +168,7 @@ struct extVisionSample { #define GPS_MAX_INTERVAL 5e5 #define BARO_MAX_INTERVAL 2e5 #define RNG_MAX_INTERVAL 2e5 +#define EV_MAX_INTERVAL 2e5 struct parameters { // measurement source control diff --git a/EKF/control.cpp b/EKF/control.cpp index 2830d696dc..8a151fc178 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -56,6 +56,25 @@ void Ekf::controlFusionModes() _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } + // external vision position aiding selection logic + if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { + // check for a exernal vision measurement that has fallen behind the fusion time horizon + if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) { + // turn on use of external vision measurements for position + _control_status.flags.ev_pos = true; + // reset the position, height and velocity + resetPosition(); + resetVelocity(); + resetHeight(); + } + } + + // external vision yaw aiding selection logic + if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align + && (_time_last_imu - _time_last_ext_vision) < 5e5) { + //TODO + } + // optical flow fusion mode selection logic // to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5578f38ec6..9e7862ff83 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -390,7 +390,11 @@ bool Ekf::initialiseFilter(void) // set the default height source from the adjustable parameter if (_hgt_counter == 0) { - _primary_hgt_source = _params.vdist_sensor_type; + if (_params.fusion_mode & MASK_USE_EVPOS) { + _primary_hgt_source = VDIST_SENSOR_EV; + } else { + _primary_hgt_source = _params.vdist_sensor_type; + } } if (_primary_hgt_source == VDIST_SENSOR_RANGE) { @@ -409,7 +413,7 @@ bool Ekf::initialiseFilter(void) } } - } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) { + } 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 // later when it passes checks. if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index d1d6d85c63..0e3ac8b008 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -51,15 +51,22 @@ // gps measurement then use for velocity initialisation bool Ekf::resetVelocity() { - // if we have a valid GPS measurement use it to initialise velocity states - gpsSample gps_newest = _gps_buffer.get_newest(); + if (_control_status.flags.gps) { + // if we have a valid GPS measurement use it to initialise velocity states + gpsSample gps_newest = _gps_buffer.get_newest(); - if (_time_last_imu - gps_newest.time_us < 400000) { - _state.vel = gps_newest.vel; + if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) { + _state.vel = gps_newest.vel; + return true; + + } else { + // XXX use the value of the last known velocity + return false; + } + } else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { + _state.vel.setZero(); return true; - } else { - // XXX use the value of the last known velocity return false; } } @@ -68,18 +75,48 @@ bool Ekf::resetVelocity() // gps measurement then use for position initialisation bool Ekf::resetPosition() { - // if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay - gpsSample gps_newest = _gps_buffer.get_newest(); + if (_control_status.flags.gps) { + // if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay + gpsSample gps_newest = _gps_buffer.get_newest(); - float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us); + float time_delay = 1e-6f * (float)(_imu_sample_delayed.time_us - gps_newest.time_us); + float max_time_delay = 1e-6f * (float)GPS_MAX_INTERVAL; - if (time_delay < 0.4f) { - _state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay; - _state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay; + if (time_delay < max_time_delay) { + _state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay; + _state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay; + return true; + + } else { + // XXX use the value of the last known position + return false; + } + } else if (_control_status.flags.opt_flow) { + _state.pos(0) = 0.0f; + _state.pos(1) = 0.0f; return true; + } else if (_control_status.flags.ev_pos) { + // if we have fresh data, reset the position to the measurement + extVisionSample ev_newest = _ext_vision_buffer.get_newest(); + if (_time_last_imu - ev_newest.time_us < 2*EV_MAX_INTERVAL) { + // use the most recent data if it's time offset from the fusion time horizon is smaller + int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; + int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us; + if (abs(dt_newest) < abs(dt_delayed)) { + _state.pos(0) = ev_newest.posNED(0); + _state.pos(1) = ev_newest.posNED(1); + } else { + _state.pos(0) = _ev_sample_delayed.posNED(0); + _state.pos(1) = _ev_sample_delayed.posNED(1); + } + return true; + + } else { + // XXX use the value of the last known position + return false; + } } else { - // XXX use the value of the last known position return false; } } @@ -116,13 +153,14 @@ void Ekf::resetHeight() vert_pos_reset = true; + // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup + baroSample baro_newest = _baro_buffer.get_newest(); + _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } else { // TODO: reset to last known range based estimate } - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - baroSample baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); } else if (_control_status.flags.baro_hgt) { // initialize vertical position with newest baro measurement @@ -158,13 +196,24 @@ void Ekf::resetHeight() vert_pos_reset = true; + // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup + baroSample baro_newest = _baro_buffer.get_newest(); + _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } else { // TODO: reset to last known gps based estimate } - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - baroSample baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } else if (_control_status.flags.ev_pos) { + // initialize vertical position with newest measurement + extVisionSample ev_newest = _ext_vision_buffer.get_newest(); + + if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) { + _state.pos(2) = ev_newest.posNED(2); + + } else { + // TODO: reset to last known baro based estimate + } } // reset the vertical velocity covariance values diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 1789323c56..e4c09b125f 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -248,6 +248,7 @@ protected: rangeSample _range_sample_delayed; airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; + extVisionSample _ev_sample_delayed; outputSample _output_sample_delayed; // filter output on the delayed time horizon outputSample _output_new; // filter output on the non-delayed time horizon