diff --git a/EKF/common.h b/EKF/common.h index a9cc104aa0..b1afcbd3a2 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -73,13 +73,6 @@ struct gps_message { float pdop; ///< position dilution of precision }; -struct flow_message { - uint8_t quality; ///< Quality of Flow data - Vector2f flowdata; ///< Optical flow rates about the X and Y body axes (rad/sec) - Vector3f gyrodata; ///< Gyro rates about the XYZ body axes (rad/sec) - uint32_t dt; ///< integration time of flow samples (microseconds) -}; - struct outputSample { Quatf quat_nominal; ///< nominal quaternion describing vehicle attitude Vector3f vel; ///< NED velocity estimate in earth frame (m/sec) @@ -137,8 +130,8 @@ struct airspeedSample { struct flowSample { uint8_t quality; ///< quality indicator between 0 and 255 - Vector2f flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive - Vector3f gyroXYZ; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive + Vector2f flow_xy_rad; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive + Vector3f gyro_xyz; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive float dt; ///< amount of integration time (sec) uint64_t time_us; ///< timestamp of the integration period leading edge (uSec) }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 06d9f45fcc..6025d270f7 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -133,8 +133,9 @@ void Ekf::controlFusionModes() } // We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon. - // This means we stop looking for new data until the old data has been fused. - if (!_flow_data_ready) { + // This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow, + // in this case we need to empty the buffer + if (!_flow_data_ready || !_control_status.flags.opt_flow) { _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) && (_R_to_earth(2, 2) > _params.range_cos_max_tilt); } @@ -525,11 +526,11 @@ void Ekf::controlOpticalFlowFusion() if (!flow_quality_good && !_control_status.flags.in_air) { // when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate - _flowRadXYcomp.zero(); + _flow_compensated_XY_rad.zero(); } else { // compensate for body motion to give a LOS rate - _flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0); - _flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1); + _flow_compensated_XY_rad(0) = _flow_sample_delayed.flow_xy_rad(0) - _flow_sample_delayed.gyro_xyz(0); + _flow_compensated_XY_rad(1) = _flow_sample_delayed.flow_xy_rad(1) - _flow_sample_delayed.gyro_xyz(1); } } else { // don't use this flow data and wait for the next data to arrive diff --git a/EKF/ekf.h b/EKF/ekf.h index 2f78dc46be..9501f61753 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -429,7 +429,7 @@ private: uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec) uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec) bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited - Vector2f _flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive + Vector2f _flow_compensated_XY_rad; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive // output predictor states Vector3f _delta_angle_corr; ///< delta angle correction vector (rad) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5633611c06..a2139c82e5 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -73,8 +73,8 @@ bool Ekf::resetVelocity() // we should have reliable OF measurements so // calculate X and Y body relative velocities from OF measurements Vector3f vel_optflow_body; - vel_optflow_body(0) = - range * _flowRadXYcomp(1) / _flow_sample_delayed.dt; - vel_optflow_body(1) = range * _flowRadXYcomp(0) / _flow_sample_delayed.dt; + vel_optflow_body(0) = - range * _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt; + vel_optflow_body(1) = range * _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt; vel_optflow_body(2) = 0.0f; // rotate from body to earth frame diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 398e608dcf..00c16d44fd 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -336,8 +336,7 @@ void EstimatorInterface::setRangeData(const rangeSample& range_sample) } } -// TODO: Change pointer to constant reference -void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow) +void EstimatorInterface::setOpticalFlowData(const flowSample& flow) { if (!_initialised || _flow_buffer_fail) { return; @@ -355,10 +354,10 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl } // limit data rate to prevent data being lost - if ((time_usec - _time_last_optflow) > _min_obs_interval_us) { + if ((flow.time_us - _time_last_optflow) > _min_obs_interval_us) { // check if enough integration time and fail if integration time is less than 50% // of min arrival interval because too much data is being lost - float delta_time = 1e-6f * (float)flow->dt; // in seconds + float delta_time = flow.dt; // in seconds const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us; bool delta_time_good = delta_time >= delta_time_min; @@ -368,7 +367,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // check magnitude is within sensor limits // use this to prevent use of a saturated flow sensor // when there are other aiding sources available - const float flow_rate_magnitude = flow->flowdata.norm() / delta_time; + const float flow_rate_magnitude = flow.flow_xy_rad.norm() / delta_time; flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate); } else { // protect against overflow caused by division with very small delta_time @@ -377,25 +376,22 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel; - const bool flow_quality_good = (flow->quality >= _params.flow_qual_min); + const bool flow_quality_good = (flow.quality >= _params.flow_qual_min); // Check data validity and write to buffers // Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion() bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow); if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) { - flowSample optflow_sample_new; - // calculate the system time-stamp for the trailing edge of the flow data integration period - optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000; - optflow_sample_new.quality = flow->quality; + _time_last_optflow = flow.time_us; - // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate - // is produced by a RH rotation of the image about the sensor axis. - optflow_sample_new.gyroXYZ = - flow->gyrodata; - optflow_sample_new.flowRadXY = -flow->flowdata; + flowSample optflow_sample_new = flow; + + // compensate time-stamp for the trailing edge of the flow data integration period + optflow_sample_new.time_us -= _params.flow_delay_ms * 1000; + optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; optflow_sample_new.dt = delta_time; - _time_last_optflow = time_usec; _flow_buffer.push(optflow_sample_new); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index c45d725e26..2ebeb30f66 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -185,8 +185,8 @@ public: void setRangeData(const rangeSample& range_sample); - // if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead - void setOpticalFlowData(uint64_t time_usec, flow_message *flow); + // if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead + void setOpticalFlowData(const flowSample& flow); // set external vision position and attitude data void setExtVisionData(const extVisionSample& evdata); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index b1fb3cf289..695a25f124 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -73,8 +73,8 @@ void Ekf::fuseOptFlow() const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; // calculate the velocity of the sensor relative to the imu in body frame - // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; + // Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign + const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body; // calculate the velocity of the sensor in the earth frame const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; @@ -102,8 +102,8 @@ void Ekf::fuseOptFlow() // correct for gyro bias errors in the data used to do the motion compensation // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. Vector2f opt_flow_rate; - opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); - opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); + opt_flow_rate(0) = _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); + opt_flow_rate(1) = _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); if (opt_flow_rate.norm() < _flow_max_rate) { _flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis @@ -522,7 +522,7 @@ bool Ekf::calcOptFlowBodyRateComp() return false; } - const bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2)); + const bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && ISFINITE(_flow_sample_delayed.gyro_xyz(2)); if (use_flow_sensor_gyro) { @@ -532,7 +532,7 @@ bool Ekf::calcOptFlowBodyRateComp() const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of)); - const Vector3f measured_body_rate(_flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt)); + const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt)); // calculate the bias estimate using a combined LPF and spike filter _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f; @@ -541,7 +541,7 @@ bool Ekf::calcOptFlowBodyRateComp() } else { // Use the EKF gyro data if optical flow sensor gyro data is not available // for clarification of the sign see definition of flowSample and imuSample in common.h - _flow_sample_delayed.gyroXYZ = -_imu_del_ang_of; + _flow_sample_delayed.gyro_xyz = -_imu_del_ang_of; _flow_gyro_bias.zero(); } diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index e0c75d1abc..db4a535fca 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -187,7 +187,7 @@ void Ekf::fuseFlowForTerrain() // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed // correct for gyro bias errors in the data used to do the motion compensation // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. - const Vector2f opt_flow_rate = Vector2f{_flowRadXYcomp} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias}; + const Vector2f opt_flow_rate = Vector2f{_flow_compensated_XY_rad} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias}; // get latest estimated orientation const float q0 = _state.quat_nominal(0); @@ -205,8 +205,8 @@ void Ekf::fuseFlowForTerrain() const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; // calculate the velocity of the sensor relative to the imu in body frame - // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; + // Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign + const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body; // calculate the velocity of the sensor in the earth frame const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; diff --git a/test/sensor_simulator/flow.cpp b/test/sensor_simulator/flow.cpp index 8c2328cbcc..e9e4544297 100644 --- a/test/sensor_simulator/flow.cpp +++ b/test/sensor_simulator/flow.cpp @@ -16,21 +16,22 @@ Flow::~Flow() void Flow::send(uint64_t time) { _flow_data.dt = time - _time_last_data_sent; - _ekf->setOpticalFlowData(time, &_flow_data); + _flow_data.time_us = time; + _ekf->setOpticalFlowData(_flow_data); } -void Flow::setData(const flow_message& flow) +void Flow::setData(const flowSample& flow) { _flow_data = flow; } -flow_message Flow::dataAtRest() +flowSample Flow::dataAtRest() { - flow_message _flow_at_rest; - _flow_at_rest.dt = 20000; - _flow_at_rest.flowdata = Vector2f{0.0f, 0.0f}; - _flow_at_rest.gyrodata = Vector3f{0.0f, 0.0f, 0.0f}; + flowSample _flow_at_rest; + _flow_at_rest.dt = 0.02f; + _flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f}; + _flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f}; _flow_at_rest.quality = 255; return _flow_at_rest; } diff --git a/test/sensor_simulator/flow.h b/test/sensor_simulator/flow.h index 947c9c815d..9dca908093 100644 --- a/test/sensor_simulator/flow.h +++ b/test/sensor_simulator/flow.h @@ -50,11 +50,11 @@ public: Flow(std::shared_ptr ekf); ~Flow(); - void setData(const flow_message& flow); - flow_message dataAtRest(); + void setData(const flowSample& flow); + flowSample dataAtRest(); private: - flow_message _flow_data; + flowSample _flow_data; void send(uint64_t time) override; diff --git a/test/sensor_simulator/range_finder.h b/test/sensor_simulator/range_finder.h index a53dc04caa..3b034d4820 100644 --- a/test/sensor_simulator/range_finder.h +++ b/test/sensor_simulator/range_finder.h @@ -51,7 +51,6 @@ public: ~RangeFinder(); void setData(float range_data, int8_t range_quality); - flow_message dataAtRest(); private: rangeSample _range_sample {}; diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index d3a5cd1bd9..11c1783c51 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -161,8 +161,7 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // WHEN: Flow data is not send and we enable flow fusion _sensor_simulator.stopFlow(); - _sensor_simulator.runSeconds(3); // TODO: without this line tests fail - // probably there are still values in the buffer. + _sensor_simulator.runSeconds(1); // empty buffer _ekf_wrapper.enableFlowFusion(); _sensor_simulator.runSeconds(3);