From 752b89b99811e27082be8147c7ff8426f0199478 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:24:54 +0200 Subject: parse hil_optical_flow message publish to flow and range finder topic --- src/modules/mavlink/mavlink_receiver.cpp | 54 +++++++++++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 55 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd789..45ea0e168 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -102,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), + _range_pub(-1), _offboard_control_sp_pub(-1), _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), @@ -200,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_state_quaternion(msg); break; + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + default: break; } @@ -363,6 +369,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) } } +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); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } + + struct range_finder_report r; + memset(&r, 0, sizeof(f)); + + r.timestamp = hrt_absolute_time(); + r.error_count = 0; + r.type = RANGE_FINDER_TYPE_LASER; + r.distance = flow.ground_distance; + r.minimum_distance = 0.0f; + r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable + r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + + if (_range_pub < 0) { + _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + + } else { + orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + } +} + void MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) { @@ -444,7 +496,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) vision_position.vx = 0.0f; vision_position.vy = 0.0f; vision_position.vz = 0.0f; - + math::Quaternion q; q.from_euler(pos.roll, pos.pitch, pos.yaw); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 91125736c..c4e12d8d8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); @@ -142,6 +143,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; + orb_advert_t _range_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; -- cgit v1.2.3