mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 16:03:34 +08:00
optical flow sensor pipeline overhaul
- 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
co-authored by
alexklimaj
alexklimaj
parent
32544452f0
commit
d5839e2dd5
@@ -104,9 +104,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 Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; }
|
||||
|
||||
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; }
|
||||
|
||||
void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
|
||||
void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
|
||||
|
||||
+32
-12
@@ -1426,14 +1426,18 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
||||
void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -71,13 +71,11 @@
|
||||
#include <uORB/topics/estimator_event_flags.h>
|
||||
#include <uORB/topics/estimator_gps_status.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
#include <uORB/topics/estimator_optical_flow_vel.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/estimator_states.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/estimator_status_flags.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/sensor_selection.h>
|
||||
@@ -91,11 +89,12 @@
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_magnetometer.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/vehicle_optical_flow.h>
|
||||
#include <uORB/topics/vehicle_optical_flow_vel.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
#include <uORB/topics/yaw_estimator_status.h>
|
||||
|
||||
|
||||
extern pthread_mutex_t ekf2_module_mutex;
|
||||
|
||||
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
|
||||
@@ -276,17 +275,18 @@ private:
|
||||
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
|
||||
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
|
||||
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
|
||||
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
|
||||
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
hrt_abstime _last_range_sensor_update{0};
|
||||
int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
|
||||
unsigned _distance_sensor_last_generation{0};
|
||||
|
||||
@@ -295,8 +295,6 @@ private:
|
||||
hrt_abstime _last_event_flags_publish{0};
|
||||
hrt_abstime _last_status_flags_publish{0};
|
||||
|
||||
hrt_abstime _last_range_sensor_update{0};
|
||||
|
||||
uint32_t _filter_control_status{0};
|
||||
uint32_t _filter_fault_status{0};
|
||||
uint32_t _innov_check_fail_status{0};
|
||||
@@ -314,12 +312,12 @@ private:
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
|
||||
uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
|
||||
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
|
||||
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
|
||||
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
|
||||
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
|
||||
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
|
||||
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
|
||||
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
|
||||
|
||||
Reference in New Issue
Block a user