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

mavlink_receiver: decode obstacle_distance message

Browse Source
This commit is contained in:
Martina
2018-02-16 14:05:48 +01:00
committed by Lorenz Meier
parent 883fb8ce57
commit f932217fdc
2 changed files with 30 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -128,6 +128,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(nullptr),
_rc_pub(nullptr),
_manual_pub(nullptr),
_obstacle_distance_pub(nullptr),
_land_detector_pub(nullptr),
_time_offset_pub(nullptr),
_follow_target_pub(nullptr),
@@ -321,6 +322,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_play_tune(msg);
break;
case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
handle_message_obstacle_distance(msg);
break;
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
handle_message_named_value_float(msg);
break;
@@ -1480,6 +1485,28 @@ MavlinkReceiver::handle_message_play_tune(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
{
mavlink_obstacle_distance_t mavlink_obstacle_distance;
mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance);
obstacle_distance_s obstacle_distance = {};
obstacle_distance.timestamp = hrt_absolute_time();
obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type;
memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances));
obstacle_distance.increment = mavlink_obstacle_distance.increment;
obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance;
obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance;
if (_obstacle_distance_pub == nullptr) {
_obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &obstacle_distance);
} else {
orb_publish(ORB_ID(obstacle_distance), _obstacle_distance_pub, &obstacle_distance);
}
}
switch_pos_t
MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
{

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

@@ -77,6 +77,7 @@
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/obstacle_distance.h>
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
@@ -155,6 +156,7 @@ private:
void handle_message_serial_control(mavlink_message_t *msg);
void handle_message_logging_ack(mavlink_message_t *msg);
void handle_message_play_tune(mavlink_message_t *msg);
void handle_message_obstacle_distance(mavlink_message_t *msg);
void handle_message_named_value_float(mavlink_message_t *msg);
void handle_message_debug(mavlink_message_t *msg);
void handle_message_debug_vect(mavlink_message_t *msg);
@@ -236,6 +238,7 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
orb_advert_t _obstacle_distance_pub;
orb_advert_t _land_detector_pub;
orb_advert_t _time_offset_pub;
orb_advert_t _follow_target_pub;