diff options
author | Helen Oleynikova <helenoleynikova@gmail.com> | 2014-04-10 09:08:37 +0200 |
---|---|---|
committer | Helen Oleynikova <helenoleynikova@gmail.com> | 2014-04-10 09:08:37 +0200 |
commit | e99291d825cfc89c67cadd8487e568d3b21d218f (patch) | |
tree | 3a055c06b89b4ad763a0c5aa6a8f6fe0981e35f8 /src/modules | |
parent | b1e4b34a56bfaaa98adb253195d9751f960d5a94 (diff) | |
download | px4-firmware-e99291d825cfc89c67cadd8487e568d3b21d218f.tar.gz px4-firmware-e99291d825cfc89c67cadd8487e568d3b21d218f.tar.bz2 px4-firmware-e99291d825cfc89c67cadd8487e568d3b21d218f.zip |
Added vicon stream.
Diffstat (limited to 'src/modules')
-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 47603b390..37929edac 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -640,6 +640,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: @@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamViconPositionEstimate(), nullptr }; |