yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

added different distance_sensor_pub pointers for flow, hil and lidar

Browse Source
This commit is contained in:
ChristophTobler
2016-02-09 15:28:04 +01:00
committed by Lorenz Meier
parent f2af55d92f
commit 519a7e2c8e
2 changed files with 12 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -110,6 +110,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_battery_pub(nullptr),
_cmd_pub(nullptr),
_flow_pub(nullptr),
_hil_distance_sensor_pub(nullptr),
_flow_distance_sensor_pub(nullptr),
_distance_sensor_pub(nullptr),
_offboard_control_mode_pub(nullptr),
_actuator_controls_pub(nullptr),
@@ -462,15 +464,15 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = 1;
d.id = 0;
d.id = MAV_DISTANCE_SENSOR_ULTRASOUND;
d.orientation = 8;
d.covariance = 0.0;
if (_distance_sensor_pub == nullptr) {
_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
if (_flow_distance_sensor_pub == nullptr) {
_flow_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);
orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d);
}
}
@@ -517,11 +519,11 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
d.orientation = 8;
d.covariance = 0.0;
if (_distance_sensor_pub == nullptr) {
_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
if (_hil_distance_sensor_pub == nullptr) {
_hil_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);
orb_publish(ORB_ID(distance_sensor), _hil_distance_sensor_pub, &d);
}
}
@@ -574,7 +576,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
d.max_distance = float(dist_sensor.max_distance) * 1e-2f; /* cm to m */
d.current_distance = float(dist_sensor.current_distance) * 1e-2f; /* cm to m */
d.type = dist_sensor.type;
d.id = dist_sensor.id;
d.id = MAV_DISTANCE_SENSOR_LASER;
d.orientation = dist_sensor.orientation;
d.covariance = dist_sensor.covariance;

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

@@ -178,6 +178,8 @@ private:
orb_advert_t _battery_pub;
orb_advert_t _cmd_pub;
orb_advert_t _flow_pub;
orb_advert_t _hil_distance_sensor_pub;
orb_advert_t _flow_distance_sensor_pub;
orb_advert_t _distance_sensor_pub;
orb_advert_t _offboard_control_mode_pub;
orb_advert_t _actuator_controls_pub;