Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

optical flow sensor pipeline overhaul

Browse Source 
- all sources of optical flow publish sensor_optical_flow
 - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow

Co-authored-by: alexklimaj <alex@arkelectron.com>
This commit is contained in:
Daniel Agar
2022-06-17 16:02:09 -04:00
parent 32544452f0
commit d5839e2dd5
49 changed files with 1415 additions and 634 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+32 -12
View File
@@ -1426,14 +1426,18 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
{
if (_ekf.getFlowCompensated().longerThan(0.f)) {
estimator_optical_flow_vel_s flow_vel{};
vehicle_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
_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.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate);
_ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral);
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_optical_flow_vel_pub.publish(flow_vel);
@@ -1711,29 +1715,29 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF flow sample
bool new_optical_flow = false;
const unsigned last_generation = _optical_flow_sub.get_last_generation();
optical_flow_s optical_flow;
const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation();
vehicle_optical_flow_s optical_flow;
if (_optical_flow_sub.update(&optical_flow)) {
if (_vehicle_optical_flow_sub.update(&optical_flow)) {
if (_msg_missed_optical_flow_perf == nullptr) {
_msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed");
} else if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
} else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_optical_flow_perf);
}
flowSample flow {
.time_us = optical_flow.timestamp,
.time_us = optical_flow.timestamp_sample,
// 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_x_integral, -optical_flow.pixel_flow_y_integral},
.gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral},
.dt = 1e-6f * (float)optical_flow.integration_timespan,
.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,
};
if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
if (PX4_ISFINITE(optical_flow.pixel_flow[0]) &&
PX4_ISFINITE(optical_flow.pixel_flow[1]) &&
flow.dt < 1) {
// Save sensor limits reported by the optical flow sensor
@@ -1745,6 +1749,22 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
new_optical_flow = true;
}
// use optical_flow distance as range sample if distance_sensor unavailable
if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) {
int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
rangeSample range_sample {
.time_us = optical_flow.timestamp_sample,
.rng = optical_flow.distance_m,
.quality = quality,
};
_ekf.setRangeData(range_sample);
// set sensor limits
_ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance);
}
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}