From f105d65ab198f42b7d39002cc9b918edf06d77a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Aug 2015 14:14:14 +0200 Subject: [PATCH] MAVLink: Only send trigger message when updated --- src/modules/mavlink/mavlink_messages.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fefdfeb7f7..6e5cc9ed24 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1094,6 +1094,7 @@ public: private: MavlinkOrbSubscription *_trigger_sub; + uint64_t _trigger_time; /* do not allow top copying this class */ MavlinkStreamCameraTrigger(MavlinkStreamCameraTrigger &); @@ -1101,19 +1102,24 @@ private: protected: explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink), - _trigger_sub(_mavlink->add_orb_subscription(ORB_ID(camera_trigger))) + _trigger_sub(_mavlink->add_orb_subscription(ORB_ID(camera_trigger))), + _trigger_time(0) {} void send(const hrt_abstime t) { struct camera_trigger_s trigger; - if (_trigger_sub->update(&trigger)) { + if (_trigger_sub->update(&_trigger_time, &trigger)) { mavlink_camera_trigger_t msg; msg.time_usec = trigger.timestamp; + msg.seq = trigger.seq; - _mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg); + /* ensure that only active trigger events are sent */ + if (trigger.timestamp > 0) { + _mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg); + } } } };