fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 15:41: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-20 20:56:56 -04:00
co-authored by alexklimaj
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/mavlink/mavlink_receiver.cpp
+72 -111
View File
@@ -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)