diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-20 02:04:05 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-20 02:04:05 +0200 |
commit | 2ea32b315fb06f13813950a8ef385dd521694278 (patch) | |
tree | 2ea9711c8a3f18244685a6a356bfc43b3836730b /src/modules/mavlink/mavlink_messages.cpp | |
parent | eb9daf097e10f434e729581b3ffce6c1b322b927 (diff) | |
parent | 0b85c41cd19486e829d48cf9e604fbc25b9e52a6 (diff) | |
download | px4-firmware-2ea32b315fb06f13813950a8ef385dd521694278.tar.gz px4-firmware-2ea32b315fb06f13813950a8ef385dd521694278.tar.bz2 px4-firmware-2ea32b315fb06f13813950a8ef385dd521694278.zip |
Merged master
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 42 |
1 files changed, 42 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e608bf787..58af1fcb6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -641,6 +641,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } +}; + + class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: @@ -1344,5 +1385,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), new MavlinkStreamDistanceSensor(), + new MavlinkStreamViconPositionEstimate(), nullptr }; |