fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: added multi advertise for distance_sensor in mavlink receiver

Browse Source
This commit is contained in:
TSC21 2015-05-26 23:32:19 +01:00
parent f13510e8a6
commit 0d1bc2cd0b
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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);

1
src/modules/mavlink/mavlink_receiver.h
View File

@ -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 &);