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

distance_sensor: updated distance_sensor stream sub

Browse Source
This commit is contained in:
TSC21
2015-05-20 00:54:36 +01:00
parent 30218b0de7
commit 796b105382
1 changed files with 25 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -69,6 +69,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
@@ -2179,7 +2180,6 @@ protected:
}
};
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
@@ -2205,12 +2205,12 @@ public:
unsigned get_size()
{
return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
MavlinkOrbSubscription *_range_sub;
uint64_t _range_time;
MavlinkOrbSubscription *_distance_sensor_sub;
uint64_t _dist_sensor_time;
/* do not allow top copying this class */
MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
@@ -2218,36 +2218,44 @@ private:
protected:
explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink),
_range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))),
_range_time(0)
_distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))),
_dist_sensor_time(0)
{}
void send(const hrt_abstime t)
{
struct range_finder_report range;
struct distance_sensor_s dist_sensor;
if (_range_sub->update(&_range_time, &range)) {
if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) {
mavlink_distance_sensor_t msg;
msg.time_boot_ms = range.timestamp / 1000;
msg.time_boot_ms = dist_sensor.time_boot_ms;
switch (range.type) {
case RANGE_FINDER_TYPE_LASER:
switch (dist_sensor.type) {
case MAV_DISTANCE_SENSOR_ULTRASOUND:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
case MAV_DISTANCE_SENSOR_LASER:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
/*case MAV_DISTANCE_SENSOR_INFRARED:
msg.type = MAV_DISTANCE_SENSOR_INFRARED;
break;*/ //TODO: update mavlink def
default:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
}
msg.id = 0;
msg.orientation = 0;
msg.min_distance = range.minimum_distance * 100;
msg.max_distance = range.maximum_distance * 100;
msg.current_distance = range.distance * 100;
msg.covariance = 20;
msg.id = dist_sensor.id;
msg.orientation = dist_sensor.orientation;
msg.min_distance = dist_sensor.min_distance;
msg.max_distance = dist_sensor.max_distance;
msg.current_distance = dist_sensor.current_distance;
msg.covariance = dist_sensor.covariance;
_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
}