fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 18:14:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update optical flow interface

Browse Source
This commit is contained in:
kamilritz 2020-01-23 17:46:21 +01:00 committed by Roman Bapst
parent cafb1400fb
commit f3d790a664
12 changed files with 46 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/common.h
View File

@ -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)
};

11
EKF/control.cpp
View File

@ -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

2
EKF/ekf.h
View File

@ -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)

4
EKF/ekf_helper.cpp
View File

@ -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

26
EKF/estimator_interface.cpp
View File

@ -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);
}

4
EKF/estimator_interface.h
View File

@ -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);

14
EKF/optflow_fusion.cpp
View File

@ -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();
}

6
EKF/terrain_estimator.cpp
View File

@ -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;

15
test/sensor_simulator/flow.cpp
View File

@ -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;
}

6
test/sensor_simulator/flow.h
View File

@ -50,11 +50,11 @@ public:
Flow(std::shared_ptr<Ekf> 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;

1
test/sensor_simulator/range_finder.h
View File

@ -51,7 +51,6 @@ public:
~RangeFinder();
void setData(float range_data, int8_t range_quality);
flow_message dataAtRest();
private:
rangeSample _range_sample {};

3
test/test_EKF_fusionLogic.cpp
View File

@ -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);