forked from Archive/PX4-Autopilot
ekf2-flow: refactor control logic
Use flow rates instead of integrals in backend. This allows us to delay the data to the mitpoint integration time and simplifies the code in general. Gyro compensation can still be done in EKF2 if needed, but the flow module normally already appends the correct gyro data to the flow message.
This commit is contained in:
bresch
Daniel Agar
parent
5f8ecd6b40
commit
1c3a1183c8
|
|
@ -4,14 +4,12 @@ uint64 timestamp_sample # the timestamp of the raw data (microsec
|
|||
float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
|
||||
float32[2] vel_ne # same as vel_body but in local frame (m/s)
|
||||
|
||||
float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad)
|
||||
float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad)
|
||||
float32[2] flow_rate_uncompensated # integrated optical flow measurement (rad/s)
|
||||
float32[2] flow_rate_compensated # integrated optical flow measurement compensated for angular motion (rad/s)
|
||||
|
||||
float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s)
|
||||
float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
|
||||
|
||||
float32[3] gyro_bias
|
||||
float32[3] ref_gyro
|
||||
float32[3] meas_gyro
|
||||
|
||||
# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel
|
||||
|
|
|
|||
|
|
@ -225,11 +225,10 @@ struct airspeedSample {
|
|||
};
|
||||
|
||||
struct flowSample {
|
||||
uint64_t time_us{}; ///< timestamp of the integration period leading edge (uSec)
|
||||
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)
|
||||
uint8_t quality{}; ///< quality indicator between 0 and 255
|
||||
uint64_t time_us{}; ///< timestamp of the integration period midpoint (uSec)
|
||||
Vector2f flow_rate{}; ///< measured angular rate of the image about the X and Y body axes (rad/s), RH rotation is positive
|
||||
Vector3f gyro_rate{}; ///< measured angular rate of the inertial frame about the body axes obtained from rate gyro measurements (rad/s), RH rotation is positive
|
||||
uint8_t quality{}; ///< quality indicator between 0 and 255
|
||||
};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
|
|
|||
|
|
@ -132,14 +132,12 @@ public:
|
|||
const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
|
||||
const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
|
||||
|
||||
const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; }
|
||||
const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; }
|
||||
const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; }
|
||||
const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; }
|
||||
|
||||
const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
|
||||
const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
|
||||
const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_rate; }
|
||||
const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; }
|
||||
const Vector3f &getRefBodyRate() const { return _ref_body_rate; }
|
||||
const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; }
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
float getHeadingInnov() const
|
||||
|
|
@ -600,14 +598,9 @@ private:
|
|||
Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
|
||||
Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s)
|
||||
Vector2f _flow_vel_ne{}; ///< velocity from corrected flow measurement (local frame) (m/s)
|
||||
Vector3f _imu_del_ang_of{}; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
|
||||
Vector3f _ref_body_rate{};
|
||||
Vector3f _measured_body_rate{};
|
||||
|
||||
float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
|
||||
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)
|
||||
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
|
||||
Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
|
||||
|
||||
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
|
@ -879,6 +872,8 @@ private:
|
|||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// control fusion of optical flow observations
|
||||
void controlOpticalFlowFusion(const imuSample &imu_delayed);
|
||||
void startFlowFusion();
|
||||
void resetFlowFusion();
|
||||
void stopFlowFusion();
|
||||
|
||||
void updateOnGroundMotionForOpticalFlowChecks();
|
||||
|
|
@ -888,8 +883,7 @@ private:
|
|||
float calcOptFlowMeasVar(const flowSample &flow_sample);
|
||||
|
||||
// calculate optical flow body angular rate compensation
|
||||
// returns false if bias corrected body rate data is unavailable
|
||||
bool calcOptFlowBodyRateComp();
|
||||
void calcOptFlowBodyRateComp(const imuSample &imu_delayed);
|
||||
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void updateOptFlow(estimator_aid_source2d_s &aid_src);
|
||||
|
|
|
|||
|
|
@ -120,7 +120,7 @@ public:
|
|||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_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
|
||||
// if optical flow sensor gyro delta angles are not available, set gyro_rate vector fields to NaN and the EKF will use its internal gyro data instead
|
||||
void setOpticalFlowData(const flowSample &flow);
|
||||
|
||||
// set sensor limitations reported by the optical flow sensor
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
|
|||
// 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 = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
|
||||
const Vector2f opt_flow_rate = _flow_rate_compensated;
|
||||
|
||||
// compute the velocities in body and local frames from corrected optical flow measurement for logging only
|
||||
_flow_vel_body(0) = -opt_flow_rate(1) * range;
|
||||
|
|
@ -182,8 +182,8 @@ Vector2f Ekf::predictFlowVelBody()
|
|||
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.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 - _flow_gyro_bias)) % pos_offset_body;
|
||||
// Note: _flow_sample_delayed.gyro_rate is the negative of the body angular velocity, thus use minus sign
|
||||
const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_rate - _flow_gyro_bias)) % 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;
|
||||
|
|
@ -192,43 +192,6 @@ Vector2f Ekf::predictFlowVelBody()
|
|||
return _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
|
||||
}
|
||||
|
||||
|
||||
// calculate optical flow body angular rate compensation
|
||||
// returns false if bias corrected body rate data is unavailable
|
||||
bool Ekf::calcOptFlowBodyRateComp()
|
||||
{
|
||||
bool is_body_rate_comp_available = false;
|
||||
|
||||
if ((_delta_time_of > FLT_EPSILON)
|
||||
&& (_flow_sample_delayed.dt > FLT_EPSILON)) {
|
||||
const Vector3f reference_body_rate = -_imu_del_ang_of / _delta_time_of; // flow gyro has opposite sign convention
|
||||
_ref_body_rate = reference_body_rate;
|
||||
|
||||
if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1))) {
|
||||
_flow_sample_delayed.gyro_xyz = reference_body_rate * _flow_sample_delayed.dt;
|
||||
|
||||
} else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
|
||||
// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
|
||||
_flow_sample_delayed.gyro_xyz(2) = reference_body_rate(2) * _flow_sample_delayed.dt;
|
||||
}
|
||||
|
||||
const Vector3f measured_body_rate = _flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt;
|
||||
_measured_body_rate = measured_body_rate;
|
||||
|
||||
if (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) {
|
||||
// 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;
|
||||
}
|
||||
|
||||
is_body_rate_comp_available = true;
|
||||
}
|
||||
|
||||
// reset the accumulators
|
||||
_imu_del_ang_of.setZero();
|
||||
_delta_time_of = 0.0f;
|
||||
return is_body_rate_comp_available;
|
||||
}
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor (rad/sec)^2
|
||||
float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -41,237 +41,118 @@
|
|||
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
|
||||
{
|
||||
if (_flow_buffer) {
|
||||
// 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, 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 && !_hagl_sensor_status.flags.flow)) {
|
||||
_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
|
||||
}
|
||||
}
|
||||
|
||||
// Check if on ground motion is un-suitable for use of optical flow
|
||||
if (!_control_status.flags.in_air) {
|
||||
updateOnGroundMotionForOpticalFlowChecks();
|
||||
|
||||
} else {
|
||||
resetOnGroundMotionForOpticalFlowChecks();
|
||||
}
|
||||
|
||||
// Accumulate autopilot gyro data across the same time interval as the flow sensor
|
||||
const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt));
|
||||
if (_delta_time_of < 0.2f) {
|
||||
_imu_del_ang_of += delta_angle;
|
||||
_delta_time_of += imu_delayed.delta_ang_dt;
|
||||
|
||||
} else {
|
||||
// reset the accumulators if the time interval is too large
|
||||
_imu_del_ang_of = delta_angle;
|
||||
_delta_time_of = imu_delayed.delta_ang_dt;
|
||||
}
|
||||
|
||||
if (_flow_data_ready) {
|
||||
int32_t min_quality = _params.flow_qual_min;
|
||||
if (!_control_status.flags.in_air) {
|
||||
min_quality = _params.flow_qual_min_gnd;
|
||||
}
|
||||
|
||||
const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
|
||||
const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate);
|
||||
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
|
||||
|
||||
const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f);
|
||||
const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f);
|
||||
bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max;
|
||||
|
||||
if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) {
|
||||
|
||||
if (fabsf(imu_delayed.delta_ang_dt - _flow_sample_delayed.dt) < 0.1f) {
|
||||
// reset accumulators to current IMU
|
||||
_imu_del_ang_of = delta_angle;
|
||||
_delta_time_of = imu_delayed.delta_ang_dt;
|
||||
|
||||
is_delta_time_good = true;
|
||||
}
|
||||
|
||||
if (is_quality_good && !is_delta_time_good) {
|
||||
ECL_DEBUG("Optical flow: bad delta time: OF dt %.6f s (min: %.3f, max: %.3f), IMU dt %.6f s",
|
||||
(double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max,
|
||||
(double)imu_delayed.delta_ang_dt);
|
||||
}
|
||||
}
|
||||
|
||||
const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
|
||||
|
||||
// don't allow invalid flow gyro_xyz to propagate
|
||||
if (!_flow_sample_delayed.gyro_xyz.isAllFinite()) {
|
||||
_flow_sample_delayed.gyro_xyz.zero();
|
||||
}
|
||||
|
||||
if (is_quality_good
|
||||
&& is_magnitude_good
|
||||
&& is_tilt_good
|
||||
&& is_body_rate_comp_available
|
||||
&& is_delta_time_good) {
|
||||
// compensate for body motion to give a LOS rate
|
||||
_flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
|
||||
|
||||
} else if (!_control_status.flags.in_air) {
|
||||
|
||||
if (!is_delta_time_good) {
|
||||
// handle special case of SITL and PX4Flow where dt is forced to
|
||||
// zero when the quaity is 0
|
||||
_flow_sample_delayed.dt = delta_time_min;
|
||||
}
|
||||
|
||||
// when on the ground with poor flow quality,
|
||||
// assume zero ground relative velocity and LOS rate
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
|
||||
} else {
|
||||
// don't use this flow data and wait for the next data to arrive
|
||||
_flow_data_ready = false;
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
}
|
||||
|
||||
updateOptFlow(_aid_src_optical_flow);
|
||||
|
||||
} else {
|
||||
_flow_compensated_XY_rad.setZero();
|
||||
_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
|
||||
}
|
||||
|
||||
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
|
||||
if (_flow_data_ready) {
|
||||
const int32_t min_quality = _control_status.flags.in_air
|
||||
? _params.flow_qual_min
|
||||
: _params.flow_qual_min_gnd;
|
||||
|
||||
const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
|
||||
const bool is_magnitude_good = _flow_sample_delayed.flow_rate.isAllFinite()
|
||||
&& !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
|
||||
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
|
||||
|
||||
if (is_quality_good
|
||||
&& is_magnitude_good
|
||||
&& is_tilt_good) {
|
||||
// compensate for body motion to give a LOS rate
|
||||
calcOptFlowBodyRateComp(imu_delayed);
|
||||
_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
|
||||
|
||||
} else {
|
||||
// don't use this flow data and wait for the next data to arrive
|
||||
_flow_data_ready = false;
|
||||
_flow_rate_compensated.setZero();
|
||||
}
|
||||
}
|
||||
|
||||
if (_flow_data_ready) {
|
||||
updateOptFlow(_aid_src_optical_flow);
|
||||
|
||||
// Check if we are in-air and require optical flow to control position drift
|
||||
bool is_flow_required = _control_status.flags.in_air
|
||||
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|
||||
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
|
||||
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|
||||
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
// check if using GPS, but GPS is bad
|
||||
if (_control_status.flags.gps) {
|
||||
if (_control_status.flags.in_air && !is_flow_required) {
|
||||
// Inhibit flow use if motion is un-suitable or we have good quality GPS
|
||||
// Apply hysteresis to prevent rapid mode switching
|
||||
const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
|
||||
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
|
||||
// use a relaxed time criteria to enable it to coast through bad range finder data
|
||||
const bool terrain_available = isTerrainEstimateValid() || isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6);
|
||||
|
||||
if (_gps_error_norm > gps_err_norm_lim) {
|
||||
is_flow_required = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
|
||||
&& _control_status.flags.tilt_align
|
||||
&& (terrain_available || is_flow_required);
|
||||
|
||||
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
|
||||
const bool preflight_motion_not_ok = !_control_status.flags.in_air
|
||||
&& ((_time_delayed_us > (_time_good_motion_us + (uint64_t)1E5))
|
||||
|| (_time_delayed_us < (_time_bad_motion_us + (uint64_t)5E6)));
|
||||
const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
|
||||
|
||||
const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
|
||||
|| !_control_status.flags.tilt_align;
|
||||
|
||||
// Handle cases where we are using optical flow but we should not use it anymore
|
||||
if (_control_status.flags.opt_flow) {
|
||||
if (!(_params.flow_ctrl == 1)
|
||||
|| inhibit_flow_use) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseOptFlow();
|
||||
|
||||
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
|
||||
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
|
||||
if (is_flow_required) {
|
||||
resetFlowFusion();
|
||||
|
||||
} else {
|
||||
stopFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
stopFlowFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
// optical flow fusion mode selection logic
|
||||
if ((_params.flow_ctrl == 1) // optical flow has been selected by the user
|
||||
&& !_control_status.flags.opt_flow // we are not yet using flow data
|
||||
&& !inhibit_flow_use) {
|
||||
|
||||
// set the flag and reset the fusion timeout
|
||||
ECL_INFO("starting optical flow fusion");
|
||||
|
||||
// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
|
||||
if (!isHorizontalAidingActive()) {
|
||||
ECL_INFO("reset velocity to flow");
|
||||
_information_events.flags.reset_vel_to_flow = true;
|
||||
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
|
||||
|
||||
// reset position, estimate is relative to initial position in this mode, so we start with zero error
|
||||
if (!_control_status.flags.in_air) {
|
||||
ECL_INFO("reset position to zero");
|
||||
resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
|
||||
} else {
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
|
||||
}
|
||||
}
|
||||
|
||||
_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
_control_status.flags.opt_flow = true;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (_control_status.flags.opt_flow) {
|
||||
// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
|
||||
if (_time_delayed_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
|
||||
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
|
||||
// but use a relaxed time criteria to enable it to coast through bad range finder data
|
||||
if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6)) {
|
||||
fuseOptFlow();
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
}
|
||||
|
||||
_flow_data_ready = false;
|
||||
}
|
||||
|
||||
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
|
||||
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
|
||||
&& !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
|
||||
|
||||
ECL_INFO("reset velocity to flow");
|
||||
_information_events.flags.reset_vel_to_flow = true;
|
||||
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
|
||||
|
||||
// reset position, estimate is relative to initial position in this mode, so we start with zero error
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
|
||||
|
||||
_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) {
|
||||
|
||||
} else if (_control_status.flags.opt_flow && isTimedOut(_flow_sample_delayed.time_us, _params.reset_timeout_max)) {
|
||||
stopFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateOnGroundMotionForOpticalFlowChecks()
|
||||
void Ekf::startFlowFusion()
|
||||
{
|
||||
// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
|
||||
const float accel_norm = _accel_vec_filt.norm();
|
||||
ECL_INFO("starting optical flow fusion");
|
||||
|
||||
const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|
||||
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|
||||
|| (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|
||||
|| (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively
|
||||
if (!_aid_src_optical_flow.innovation_rejected && isHorizontalAidingActive()) {
|
||||
// Consistent with the current velocity state, simply fuse the data without reset
|
||||
fuseOptFlow();
|
||||
_control_status.flags.opt_flow = true;
|
||||
|
||||
if (motion_is_excessive) {
|
||||
_time_bad_motion_us = _time_delayed_us;
|
||||
} else if (!isHorizontalAidingActive()) {
|
||||
resetFlowFusion();
|
||||
_control_status.flags.opt_flow = true;
|
||||
|
||||
} else {
|
||||
_time_good_motion_us = _time_delayed_us;
|
||||
ECL_INFO("optical flow fusion failed to start");
|
||||
_control_status.flags.opt_flow = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
||||
void Ekf::resetFlowFusion()
|
||||
{
|
||||
_time_bad_motion_us = 0;
|
||||
_time_good_motion_us = _time_delayed_us;
|
||||
ECL_INFO("reset velocity to flow");
|
||||
_information_events.flags.reset_vel_to_flow = true;
|
||||
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
|
||||
|
||||
// reset position, estimate is relative to initial position in this mode, so we start with zero error
|
||||
if (!_control_status.flags.in_air) {
|
||||
ECL_INFO("reset position to zero");
|
||||
resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
|
||||
}
|
||||
|
||||
updateOptFlow(_aid_src_optical_flow);
|
||||
_innov_check_fail_status.flags.reject_optflow_X = false;
|
||||
_innov_check_fail_status.flags.reject_optflow_Y = false;
|
||||
|
||||
_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::stopFlowFusion()
|
||||
|
|
@ -283,3 +164,24 @@ void Ekf::stopFlowFusion()
|
|||
resetEstimatorAidStatus(_aid_src_optical_flow);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
|
||||
{
|
||||
if (imu_delayed.delta_ang_dt > FLT_EPSILON) {
|
||||
_ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias(); // flow gyro has opposite sign convention
|
||||
|
||||
} else {
|
||||
_ref_body_rate.zero();
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_rate(1))) {
|
||||
_flow_sample_delayed.gyro_rate = _ref_body_rate;
|
||||
|
||||
} else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(2))) {
|
||||
// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
|
||||
_flow_sample_delayed.gyro_rate(2) = _ref_body_rate(2);
|
||||
}
|
||||
|
||||
// calculate the bias estimate using a combined LPF and spike filter
|
||||
_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f, 0.1f) * 0.01f;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,6 +58,14 @@ void Ekf::initHagl()
|
|||
|
||||
// use the ground clearance value as our uncertainty
|
||||
_terrain_var = sq(_params.rng_gnd_clearance);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
||||
void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
|
||||
|
|
@ -419,17 +427,33 @@ void Ekf::controlHaglFakeFusion()
|
|||
&& !_hagl_sensor_status.flags.range_finder
|
||||
&& !_hagl_sensor_status.flags.flow) {
|
||||
|
||||
initHagl();
|
||||
bool recent_terrain_aiding = false;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
recent_terrain_aiding |= isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)1e6);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
recent_terrain_aiding |= isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)1e6);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
|
||||
initHagl();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isTerrainEstimateValid() const
|
||||
{
|
||||
bool valid = false;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
// we have been fusing range finder measurements in the last 5 seconds
|
||||
if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
|
||||
return true;
|
||||
if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
|
||||
if (_hagl_sensor_status.flags.range_finder || !_control_status.flags.in_air) {
|
||||
valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
|
@ -439,12 +463,12 @@ bool Ekf::isTerrainEstimateValid() const
|
|||
// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
|
||||
// this can only be the case if the main filter does not fuse optical flow
|
||||
if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) {
|
||||
return true;
|
||||
valid = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
return false;
|
||||
return valid;
|
||||
}
|
||||
|
||||
void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
|
||||
|
|
|
|||
|
|
@ -2002,15 +2002,13 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp)
|
|||
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
|
||||
_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
|
||||
|
||||
_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
|
||||
_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
|
||||
_ekf.getFlowUncompensated().copyTo(flow_vel.flow_rate_uncompensated);
|
||||
_ekf.getFlowCompensated().copyTo(flow_vel.flow_rate_compensated);
|
||||
|
||||
_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate);
|
||||
_ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral);
|
||||
|
||||
_ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias);
|
||||
_ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro);
|
||||
_ekf.getMeasuredBodyRate().copyTo(flow_vel.meas_gyro);
|
||||
|
||||
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
|
||||
|
|
@ -2361,17 +2359,30 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
perf_count(_msg_missed_optical_flow_perf);
|
||||
}
|
||||
|
||||
flowSample flow {
|
||||
.time_us = optical_flow.timestamp_sample,
|
||||
const float dt = 1e-6f * (float)optical_flow.integration_timespan_us;
|
||||
Vector2f flow_rate;
|
||||
Vector3f gyro_rate;
|
||||
|
||||
if (dt > FLT_EPSILON) {
|
||||
// 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.
|
||||
.flow_xy_rad = Vector2f{-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]},
|
||||
.gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]},
|
||||
.dt = 1e-6f * (float)optical_flow.integration_timespan_us,
|
||||
.quality = optical_flow.quality,
|
||||
flow_rate = Vector2f(-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]) / dt;
|
||||
gyro_rate = Vector3f(-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]) / dt;
|
||||
|
||||
} else if (optical_flow.quality == 0) {
|
||||
// handle special case of SITL and PX4Flow where dt is forced to zero when the quaity is 0
|
||||
flow_rate.zero();
|
||||
gyro_rate.zero();
|
||||
}
|
||||
|
||||
flowSample flow {
|
||||
.time_us = optical_flow.timestamp_sample - optical_flow.integration_timespan_us / 2, // correct timestamp to midpoint of integration interval as the data is converted to rates
|
||||
.flow_rate = flow_rate,
|
||||
.gyro_rate = gyro_rate,
|
||||
.quality = optical_flow.quality
|
||||
};
|
||||
|
||||
if (Vector2f(optical_flow.pixel_flow).isAllFinite() && flow.dt < 1) {
|
||||
if (Vector2f(optical_flow.pixel_flow).isAllFinite() && optical_flow.integration_timespan_us < 1e6) {
|
||||
|
||||
// Save sensor limits reported by the optical flow sensor
|
||||
_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
|
||||
|
|
|
|||
|
|
@ -28,9 +28,8 @@ void Flow::setData(const flowSample &flow)
|
|||
flowSample Flow::dataAtRest()
|
||||
{
|
||||
flowSample flow_at_rest;
|
||||
flow_at_rest.dt = static_cast<float>(_update_period) * 1e-6f;
|
||||
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.flow_rate = Vector2f{0.0f, 0.0f};
|
||||
flow_at_rest.gyro_rate = Vector3f{0.0f, 0.0f, 0.0f};
|
||||
flow_at_rest.quality = 255;
|
||||
return flow_at_rest;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -266,11 +266,11 @@ void SensorSimulator::setSingleReplaySample(const sensor_info &sample)
|
|||
|
||||
} else if (sample.sensor_type == sensor_info::measurement_t::FLOW) {
|
||||
flowSample flow_sample;
|
||||
flow_sample.flow_xy_rad = Vector2f(sample.sensor_data[0],
|
||||
sample.sensor_data[1]);
|
||||
flow_sample.gyro_xyz = Vector3f(sample.sensor_data[2],
|
||||
sample.sensor_data[3],
|
||||
sample.sensor_data[4]);
|
||||
flow_sample.flow_rate = Vector2f(sample.sensor_data[0],
|
||||
sample.sensor_data[1]);
|
||||
flow_sample.gyro_rate = Vector3f(sample.sensor_data[2],
|
||||
sample.sensor_data[3],
|
||||
sample.sensor_data[4]);
|
||||
flow_sample.quality = sample.sensor_data[5];
|
||||
_flow.setData(flow_sample);
|
||||
|
||||
|
|
@ -373,9 +373,9 @@ void SensorSimulator::setSensorDataFromTrajectory()
|
|||
if (_flow.isRunning()) {
|
||||
flowSample flow_sample = _flow.dataAtRest();
|
||||
const Vector3f vel_body = R_world_to_body * vel_world;
|
||||
flow_sample.flow_xy_rad =
|
||||
Vector2f(vel_body(1) * flow_sample.dt / distance_to_ground,
|
||||
-vel_body(0) * flow_sample.dt / distance_to_ground);
|
||||
flow_sample.flow_rate =
|
||||
Vector2f(vel_body(1) / distance_to_ground,
|
||||
-vel_body(0) / distance_to_ground);
|
||||
_flow.setData(flow_sample);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -102,9 +102,9 @@ void EkfFlowTest::startZeroFlowFusion()
|
|||
void EkfFlowTest::setFlowFromHorizontalVelocityAndDistance(flowSample &flow_sample,
|
||||
const Vector2f &simulated_horz_velocity, float estimated_distance_to_ground)
|
||||
{
|
||||
flow_sample.flow_xy_rad =
|
||||
Vector2f(simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
|
||||
-simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
|
||||
flow_sample.flow_rate =
|
||||
Vector2f(simulated_horz_velocity(1) / estimated_distance_to_ground,
|
||||
-simulated_horz_velocity(0) / estimated_distance_to_ground);
|
||||
}
|
||||
|
||||
TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
|
||||
|
|
@ -140,6 +140,8 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
|
|||
|
||||
// THEN: estimated velocity should match simulated velocity
|
||||
const Vector3f estimated_velocity = _ekf->getVelocity();
|
||||
estimated_velocity.print();
|
||||
simulated_velocity.print();
|
||||
EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity))
|
||||
<< "estimated vel = " << estimated_velocity(0) << ", "
|
||||
<< estimated_velocity(1);
|
||||
|
|
@ -163,11 +165,11 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
|
|||
|
||||
// WHEN: start fusing flow data
|
||||
flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
|
||||
flow_sample.dt = 0.f; // some sensors force dt to zero when quality is low
|
||||
flow_sample.quality = 0;
|
||||
_sensor_simulator._flow.setData(flow_sample);
|
||||
_ekf_wrapper.enableFlowFusion();
|
||||
_sensor_simulator.startFlow();
|
||||
_sensor_simulator.startRangeFinder();
|
||||
_sensor_simulator.runSeconds(1.0);
|
||||
|
||||
// THEN: estimated velocity should match simulated velocity
|
||||
|
|
@ -248,7 +250,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
|
|||
setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
|
||||
|
||||
// use autopilot gyro data
|
||||
flow_sample.gyro_xyz.setAll(NAN);
|
||||
flow_sample.gyro_rate.setAll(NAN);
|
||||
|
||||
_sensor_simulator._flow.setData(flow_sample);
|
||||
_sensor_simulator._imu.setGyroData(body_rate);
|
||||
|
|
@ -286,7 +288,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
|
|||
|
||||
// use flow sensor gyro data
|
||||
// for clarification of the sign, see definition of flowSample
|
||||
flow_sample.gyro_xyz = -body_rate * flow_sample.dt;
|
||||
flow_sample.gyro_rate = -body_rate;
|
||||
|
||||
_sensor_simulator._flow.setData(flow_sample);
|
||||
_sensor_simulator._imu.setGyroData(body_rate);
|
||||
|
|
|
|||
|
|
@ -183,6 +183,7 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow)
|
|||
const float max_ground_distance = 50.f;
|
||||
_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
|
||||
_sensor_simulator.startFlow();
|
||||
_sensor_simulator.startRangeFinder();
|
||||
_ekf_wrapper.enableFlowFusion();
|
||||
|
||||
_ekf->set_in_air_status(true);
|
||||
|
|
|
|||
|
|
@ -192,6 +192,7 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
|
|||
_sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest());
|
||||
_ekf_wrapper.enableFlowFusion();
|
||||
_sensor_simulator.startFlow();
|
||||
_sensor_simulator.startRangeFinder();
|
||||
|
||||
_ekf_wrapper.enableGpsHeightFusion();
|
||||
|
||||
|
|
|
|||
|
|
@ -98,9 +98,9 @@ public:
|
|||
|
||||
// Configure optical flow simulator data
|
||||
flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
|
||||
flow_sample.flow_xy_rad =
|
||||
Vector2f(simulated_velocity(1) * flow_sample.dt / flow_height,
|
||||
-simulated_velocity(0) * flow_sample.dt / flow_height);
|
||||
flow_sample.flow_rate =
|
||||
Vector2f(simulated_velocity(1) / flow_height,
|
||||
-simulated_velocity(0) / flow_height);
|
||||
_sensor_simulator._flow.setData(flow_sample);
|
||||
const float max_flow_rate = 5.f;
|
||||
const float min_ground_distance = 0.f;
|
||||
|
|
|
|||
|
|
@ -290,12 +290,12 @@ void VehicleOpticalFlow::Run()
|
|||
// 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.
|
||||
const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
|
||||
const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
|
||||
const Vector3f gyro_rate_integral{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
|
||||
|
||||
const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
|
||||
|
||||
// compensate for body motion to give a LOS rate
|
||||
const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
|
||||
const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_rate_integral.xy();
|
||||
|
||||
Vector3f vel_optflow_body;
|
||||
vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
|
||||
|
|
@ -320,24 +320,19 @@ void VehicleOpticalFlow::Run()
|
|||
flow_vel.vel_ne[1] = flow_vel_ne(1);
|
||||
}
|
||||
|
||||
// flow_uncompensated_integral
|
||||
flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
|
||||
const Vector2f flow_rate(flow_xy_rad * (1.f / flow_dt));
|
||||
flow_rate.copyTo(flow_vel.flow_rate_uncompensated);
|
||||
|
||||
// flow_compensated_integral
|
||||
flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
|
||||
const Vector2f flow_rate_compensated(flow_compensated_XY_rad * (1.f / flow_dt));
|
||||
flow_rate_compensated.copyTo(flow_vel.flow_rate_compensated);
|
||||
|
||||
const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
|
||||
const Vector3f measured_body_rate(gyro_rate_integral * (1.f / flow_dt));
|
||||
|
||||
// gyro_rate
|
||||
flow_vel.gyro_rate[0] = measured_body_rate(0);
|
||||
flow_vel.gyro_rate[1] = measured_body_rate(1);
|
||||
flow_vel.gyro_rate[2] = measured_body_rate(2);
|
||||
|
||||
// gyro_rate_integral
|
||||
flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
|
||||
flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
|
||||
flow_vel.gyro_rate_integral[2] = gyro_xyz(2);
|
||||
|
||||
flow_vel.timestamp = hrt_absolute_time();
|
||||
|
||||
_vehicle_optical_flow_vel_pub.publish(flow_vel);
|
||||
|
|
|
|||
Loading…
Reference in New Issue