fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 06:30:35 +08:00
Code Issues 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/simulator/simulator.h
+2 -3
View File
@@ -64,10 +64,10 @@
#include <uORB/topics/esc_report.h>
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_optical_flow.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -158,7 +158,6 @@ private:
void check_failure_injections();
int publish_flow_topic(const mavlink_hil_optical_flow_t *flow);
int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
int publish_distance_topic(const mavlink_distance_sensor_t *dist);
@@ -191,7 +190,7 @@ private:
// uORB publisher handlers
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::PublicationMulti<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
src/modules/simulator/simulator_mavlink.cpp
+39 -45
View File
@@ -665,7 +665,45 @@ void Simulator::handle_message_optical_flow(const mavlink_message_t *msg)
{
mavlink_hil_optical_flow_t flow;
mavlink_msg_hil_optical_flow_decode(msg, &flow);
publish_flow_topic(&flow);
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = 0;
device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
sensor_optical_flow_s sensor_optical_flow{};
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
sensor_optical_flow.device_id = device_id.devid;
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
sensor_optical_flow.quality = flow.quality;
if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) {
sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro;
sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro;
sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro;
sensor_optical_flow.delta_angle_available = true;
}
sensor_optical_flow.max_flow_rate = NAN;
sensor_optical_flow.min_ground_distance = NAN;
sensor_optical_flow.max_ground_distance = NAN;
// Use distance value for distance sensor topic
if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
// Positive value (including zero): distance known. Negative value: Unknown distance.
sensor_optical_flow.distance_m = flow.distance;
sensor_optical_flow.distance_available = true;
}
sensor_optical_flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(sensor_optical_flow);
}
void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
@@ -1309,50 +1347,6 @@ void Simulator::check_failure_injections()
}
}
int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
{
optical_flow_s flow = {};
flow.sensor_id = flow_mavlink->sensor_id;
flow.timestamp = hrt_absolute_time();
flow.time_since_last_sonar_update = 0;
flow.frame_count_since_last_readout = 0; // ?
flow.integration_timespan = flow_mavlink->integration_time_us;
flow.ground_distance_m = flow_mavlink->distance;
flow.gyro_temperature = flow_mavlink->temperature;
flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
flow.quality = flow_mavlink->quality;
/* fill in sensor limits */
float flow_rate_max;
param_get(param_find("SENS_FLOW_MAXR"), &flow_rate_max);
float flow_min_hgt;
param_get(param_find("SENS_FLOW_MINHGT"), &flow_min_hgt);
float flow_max_hgt;
param_get(param_find("SENS_FLOW_MAXHGT"), &flow_max_hgt);
flow.max_flow_rate = flow_rate_max;
flow.min_ground_distance = flow_min_hgt;
flow.max_ground_distance = flow_max_hgt;
/* rotate measurements according to parameter */
int32_t flow_rot_int;
param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int);
const enum Rotation flow_rot = (Rotation)flow_rot_int;
float zeroval = 0.0f;
rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval);
rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral);
_flow_pub.publish(flow);
return PX4_OK;
}
int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
{
uint64_t timestamp = hrt_absolute_time();