diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-23 14:22:52 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-23 14:22:52 +0200 |
commit | 2998685a3ac593c7b5341f684491e5faef3cc564 (patch) | |
tree | 6a614a6f7f95e9c17160c3ce837f77c351d667cd /src/modules/mavlink/mavlink_messages.cpp | |
parent | 8634780e80f728ccbd4bc63860aeaa11727aeedb (diff) | |
parent | 60554c8a5682bc5b2edb66e1ca6b7a9163b1dbf9 (diff) | |
download | px4-firmware-2998685a3ac593c7b5341f684491e5faef3cc564.tar.gz px4-firmware-2998685a3ac593c7b5341f684491e5faef3cc564.tar.bz2 px4-firmware-2998685a3ac593c7b5341f684491e5faef3cc564.zip |
Merge branch 'mpc_local_pos' into mpc_rc
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 49 |
1 files changed, 48 insertions, 1 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 648228e3b..678ce1645 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include <uORB/topics/navigation_capabilities.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> +#include <drivers/drv_range_finder.h> #include "mavlink_messages.h" @@ -270,7 +271,7 @@ protected: status->load * 1000.0f, status->battery_voltage * 1000.0f, status->battery_current * 1000.0f, - status->battery_remaining, + status->battery_remaining * 100.0f, status->drop_rate_comm, status->errors_comm, status->errors_count1, @@ -1312,6 +1313,51 @@ protected: } }; +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() + { + return "DISTANCE_SENSOR"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + struct range_finder_report *range; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + range = (struct range_finder_report *)range_sub->get_data(); + } + + void send(const hrt_abstime t) + { + (void)range_sub->update(t); + + uint8_t type; + + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1338,6 +1384,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamDistanceSensor(), new MavlinkStreamViconPositionEstimate(), nullptr }; |