From 0d1bc2cd0bc6e57ca8ae14fa39dd4569fa30b858 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 26 May 2015 23:32:19 +0100 Subject: [PATCH] mavlink: added multi advertise for distance_sensor in mavlink receiver --- src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++++---- src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 59dfb45f47..8a9b426484 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -133,7 +133,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), - _time_offset(0) + _time_offset(0), + _orb_class_instance(-1) { } @@ -430,7 +431,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) d.covariance = 0.0; if (_distance_sensor_pub < 0) { - _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + _distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, + &_orb_class_instance, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); } @@ -480,7 +482,8 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) d.covariance = 0.0; if (_distance_sensor_pub < 0) { - _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + _distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, + &_orb_class_instance, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); } @@ -542,7 +545,8 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum if (_distance_sensor_pub < 0) { - _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + _distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, + &_orb_class_instance, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 016305e796..8fffad4c3e 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -192,6 +192,7 @@ private: struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; + int _orb_class_instance; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &);