Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+8 -4
View File
@@ -133,7 +133,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_sp{}, _att_sp{},
_rates_sp{}, _rates_sp{},
_time_offset_avg_alpha(0.6), _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; d.covariance = 0.0;
if (_distance_sensor_pub < 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 { } else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); 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; d.covariance = 0.0;
if (_distance_sensor_pub < 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 { } else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); 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 /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum
if (_distance_sensor_pub < 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 { } else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
src/modules/mavlink/mavlink_receiver.h
+1
View File
@@ -192,6 +192,7 @@ private:
struct vehicle_rates_setpoint_s _rates_sp; struct vehicle_rates_setpoint_s _rates_sp;
double _time_offset_avg_alpha; double _time_offset_avg_alpha;
uint64_t _time_offset; uint64_t _time_offset;
int _orb_class_instance;
/* do not allow copying this class */ /* do not allow copying this class */
MavlinkReceiver(const MavlinkReceiver &); MavlinkReceiver(const MavlinkReceiver &);