dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

distance_sensor: added mavlink_receiver handler for msg

This commit is contained in:
TSC21 2015-05-20 00:15:27 +01:00
parent 364492a325
commit 30218b0de7
2 changed files with 38 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -134,7 +134,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_sp{},
_rates_sp{},
_time_offset_avg_alpha(0.6),
_time_offset(0)
_time_offset(0),
_distance_sensor_pub(-1)
{
}
@ -211,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_timesync(msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor(msg);
break;
default:
break;
}
@ -495,6 +500,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
{
/* distance sensor */
mavlink_distance_sensor_t dist_sensor;
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.time_boot_ms = dist_sensor.time_boot_ms;
d.min_distance = dist_sensor.min_distance;
d.max_distance = dist_sensor.max_distance;
d.current_distance = dist_sensor.current_distance;
d.type = dist_sensor.type;
d.id = dist_sensor.id;
d.orientation = dist_sensor.orientation;
d.covariance = dist_sensor.covariance;
/// 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);
} else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
void
MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
{

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

@ -74,6 +74,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_force_setpoint.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include "mavlink_ftp.h"
@ -135,6 +136,7 @@ private:
void handle_message_hil_sensor(mavlink_message_t *msg);
void handle_message_hil_gps(mavlink_message_t *msg);
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void *receive_thread(void *arg);
@ -191,6 +193,7 @@ private:
struct vehicle_rates_setpoint_s _rates_sp;
double _time_offset_avg_alpha;
uint64_t _time_offset;
orb_advert_t _distance_sensor_pub;
/* do not allow copying this class */
MavlinkReceiver(const MavlinkReceiver &);