fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:53:33 +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/streams/OPTICAL_FLOW_RAD.hpp
+23 -17
View File
@@ -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);