mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 10:23: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
@@ -89,12 +89,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_parameters_manager(parent),
|
||||
_mavlink_timesync(parent)
|
||||
{
|
||||
_handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT");
|
||||
_handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR");
|
||||
_handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT");
|
||||
_handle_sens_flow_rot = param_find("SENS_FLOW_ROT");
|
||||
_handle_ekf2_min_rng = param_find("EKF2_MIN_RNG");
|
||||
_handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX");
|
||||
}
|
||||
|
||||
void
|
||||
@@ -795,105 +789,93 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
|
||||
void
|
||||
MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
||||
{
|
||||
/* optical flow */
|
||||
mavlink_optical_flow_rad_t flow;
|
||||
mavlink_msg_optical_flow_rad_decode(msg, &flow);
|
||||
|
||||
optical_flow_s f{};
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_MAVLINK;
|
||||
|
||||
f.timestamp = hrt_absolute_time();
|
||||
f.time_since_last_sonar_update = flow.time_delta_distance_us;
|
||||
f.integration_timespan = flow.integration_time_us;
|
||||
f.pixel_flow_x_integral = flow.integrated_x;
|
||||
f.pixel_flow_y_integral = flow.integrated_y;
|
||||
f.gyro_x_rate_integral = flow.integrated_xgyro;
|
||||
f.gyro_y_rate_integral = flow.integrated_ygyro;
|
||||
f.gyro_z_rate_integral = flow.integrated_zgyro;
|
||||
f.gyro_temperature = flow.temperature;
|
||||
f.ground_distance_m = flow.distance;
|
||||
f.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
f.max_flow_rate = _param_sens_flow_maxr;
|
||||
f.min_ground_distance = _param_sens_flow_minhgt;
|
||||
f.max_ground_distance = _param_sens_flow_maxhgt;
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
/* read flow sensor parameters */
|
||||
const Rotation flow_rot = (Rotation)_param_sens_flow_rot;
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
|
||||
/* rotate measurements according to parameter */
|
||||
float zero_val = 0.0f;
|
||||
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val);
|
||||
rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
|
||||
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||
|
||||
_flow_pub.publish(f);
|
||||
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||
sensor_optical_flow.quality = flow.quality;
|
||||
|
||||
/* Use distance value for distance sensor topic */
|
||||
if (flow.distance > 0.0f) { // negative values signal invalid data
|
||||
|
||||
distance_sensor_s d{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
|
||||
d.timestamp = f.timestamp;
|
||||
d.min_distance = _param_ekf2_min_rng;
|
||||
d.max_distance = _param_ekf2_rng_a_hmax;
|
||||
d.current_distance = flow.distance; /* both are in m */
|
||||
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
|
||||
d.device_id = device_id.devid;
|
||||
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||
d.variance = 0.01;
|
||||
d.signal_quality = -1;
|
||||
|
||||
_flow_distance_sensor_pub.publish(d);
|
||||
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
|
||||
MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
{
|
||||
/* optical flow */
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
|
||||
optical_flow_s f{};
|
||||
|
||||
f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
|
||||
f.integration_timespan = flow.integration_time_us;
|
||||
f.pixel_flow_x_integral = flow.integrated_x;
|
||||
f.pixel_flow_y_integral = flow.integrated_y;
|
||||
f.gyro_x_rate_integral = flow.integrated_xgyro;
|
||||
f.gyro_y_rate_integral = flow.integrated_ygyro;
|
||||
f.gyro_z_rate_integral = flow.integrated_zgyro;
|
||||
f.time_since_last_sonar_update = flow.time_delta_distance_us;
|
||||
f.ground_distance_m = flow.distance;
|
||||
f.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
f.gyro_temperature = flow.temperature;
|
||||
|
||||
_flow_pub.publish(f);
|
||||
|
||||
/* Use distance value for distance sensor topic */
|
||||
distance_sensor_s d{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
|
||||
|
||||
d.timestamp = hrt_absolute_time();
|
||||
d.min_distance = 0.3f;
|
||||
d.max_distance = 5.0f;
|
||||
d.current_distance = flow.distance; /* both are in m */
|
||||
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
|
||||
d.device_id = device_id.devid;
|
||||
d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||
d.variance = 0.01;
|
||||
d.signal_quality = -1;
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
_flow_distance_sensor_pub.publish(d);
|
||||
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
|
||||
@@ -934,9 +916,10 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
|
||||
distance_sensor_s ds{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
device_id.devid_s.address = dist_sensor.id;
|
||||
|
||||
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
|
||||
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
|
||||
@@ -2338,10 +2321,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
|
||||
|
||||
sensor_gps_s gps{};
|
||||
|
||||
device::Device::DeviceId device_id{};
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = _mavlink->get_instance_id();
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
|
||||
|
||||
gps.device_id = device_id.devid;
|
||||
|
||||
gps.lat = hil_gps.lat;
|
||||
@@ -3475,30 +3460,6 @@ MavlinkReceiver::updateParams()
|
||||
{
|
||||
// update parameters from storage
|
||||
ModuleParams::updateParams();
|
||||
|
||||
if (_handle_sens_flow_maxhgt != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt);
|
||||
}
|
||||
|
||||
if (_handle_sens_flow_maxr != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr);
|
||||
}
|
||||
|
||||
if (_handle_sens_flow_minhgt != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt);
|
||||
}
|
||||
|
||||
if (_handle_sens_flow_rot != PARAM_INVALID) {
|
||||
param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
|
||||
}
|
||||
|
||||
if (_handle_ekf2_min_rng != PARAM_INVALID) {
|
||||
param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng);
|
||||
}
|
||||
|
||||
if (_handle_ekf2_rng_a_hmax != PARAM_INVALID) {
|
||||
param_get(_handle_ekf2_rng_a_hmax, &_param_ekf2_rng_a_hmax);
|
||||
}
|
||||
}
|
||||
|
||||
void *MavlinkReceiver::start_trampoline(void *context)
|
||||
|
||||
@@ -86,13 +86,13 @@
|
||||
#include <uORB/topics/obstacle_distance.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/onboard_computer_status.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/ping.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/radio_status.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_optical_flow.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/transponder_report.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
@@ -308,7 +308,6 @@ private:
|
||||
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
|
||||
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
|
||||
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
|
||||
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
|
||||
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};
|
||||
@@ -331,13 +330,13 @@ private:
|
||||
|
||||
// ORB publications (multi)
|
||||
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
|
||||
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
|
||||
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping)};
|
||||
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
||||
uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
|
||||
|
||||
// ORB publications (queue length > 1)
|
||||
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
|
||||
@@ -396,20 +395,6 @@ private:
|
||||
hrt_abstime _heartbeat_component_udp_bridge{0};
|
||||
hrt_abstime _heartbeat_component_uart_bridge{0};
|
||||
|
||||
param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
|
||||
param_t _handle_sens_flow_maxr{PARAM_INVALID};
|
||||
param_t _handle_sens_flow_minhgt{PARAM_INVALID};
|
||||
param_t _handle_sens_flow_rot{PARAM_INVALID};
|
||||
param_t _handle_ekf2_min_rng{PARAM_INVALID};
|
||||
param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID};
|
||||
|
||||
float _param_sens_flow_maxhgt{-1.0f};
|
||||
float _param_sens_flow_maxr{-1.0f};
|
||||
float _param_sens_flow_minhgt{-1.0f};
|
||||
int32_t _param_sens_flow_rot{0};
|
||||
float _param_ekf2_min_rng{NAN};
|
||||
float _param_ekf2_rng_a_hmax{NAN};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
|
||||
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
#ifndef OPTICAL_FLOW_RAD_HPP
|
||||
#define OPTICAL_FLOW_RAD_HPP
|
||||
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/vehicle_optical_flow.h>
|
||||
|
||||
class MavlinkStreamOpticalFlowRad : public MavlinkStream
|
||||
{
|
||||
@@ -49,34 +49,40 @@ public:
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
|
||||
return _vehicle_optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) :
|
||||
0;
|
||||
}
|
||||
|
||||
private:
|
||||
explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {}
|
||||
|
||||
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
|
||||
uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)};
|
||||
|
||||
bool send() override
|
||||
{
|
||||
optical_flow_s flow;
|
||||
vehicle_optical_flow_s flow;
|
||||
|
||||
if (_optical_flow_sub.update(&flow)) {
|
||||
if (_vehicle_optical_flow_sub.update(&flow)) {
|
||||
mavlink_optical_flow_rad_t msg{};
|
||||
|
||||
msg.time_usec = flow.timestamp;
|
||||
msg.sensor_id = flow.sensor_id;
|
||||
msg.integrated_x = flow.pixel_flow_x_integral;
|
||||
msg.integrated_y = flow.pixel_flow_y_integral;
|
||||
msg.integrated_xgyro = flow.gyro_x_rate_integral;
|
||||
msg.integrated_ygyro = flow.gyro_y_rate_integral;
|
||||
msg.integrated_zgyro = flow.gyro_z_rate_integral;
|
||||
msg.distance = flow.ground_distance_m;
|
||||
msg.time_usec = flow.timestamp_sample;
|
||||
msg.sensor_id = _vehicle_optical_flow_sub.get_instance();
|
||||
msg.integration_time_us = flow.integration_timespan_us;
|
||||
msg.integrated_x = flow.pixel_flow[0];
|
||||
msg.integrated_y = flow.pixel_flow[1];
|
||||
msg.integrated_xgyro = flow.delta_angle[0];
|
||||
msg.integrated_ygyro = flow.delta_angle[1];
|
||||
msg.integrated_zgyro = flow.delta_angle[2];
|
||||
// msg.temperature = 0;
|
||||
msg.quality = flow.quality;
|
||||
msg.integration_time_us = flow.integration_timespan;
|
||||
msg.sensor_id = flow.sensor_id;
|
||||
msg.time_delta_distance_us = flow.time_since_last_sonar_update;
|
||||
msg.temperature = flow.gyro_temperature;
|
||||
// msg.time_delta_distance_us = 0;
|
||||
|
||||
if (PX4_ISFINITE(flow.distance_m)) {
|
||||
msg.distance = flow.distance_m;
|
||||
|
||||
} else {
|
||||
msg.distance = -1;
|
||||
}
|
||||
|
||||
mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user