diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-24 23:46:58 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-24 23:46:58 +0400 |
commit | d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0 (patch) | |
tree | 1ba840d46f4d805b0a6baabbad3450157762508a /src/modules/mavlink/mavlink_stream.cpp | |
parent | b17cdb12b013a99a924b02ccef718c0ea0f776aa (diff) | |
download | px4-firmware-d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0.tar.gz px4-firmware-d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0.tar.bz2 px4-firmware-d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0.zip |
mavlink: major rewrite, prepare for dynamic mavlink streams configuration, WIP
Diffstat (limited to 'src/modules/mavlink/mavlink_stream.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_stream.cpp | 48 |
1 files changed, 48 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp new file mode 100644 index 000000000..2a6728fdb --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -0,0 +1,48 @@ +/* + * mavlink_stream.cpp + * + * Created on: 24.02.2014 + * Author: ton + */ + +#include <stdlib.h> + +#include "mavlink_orb_listener.h" + +MavlinkStream::MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +{ + this->callback = callback; + this->arg = arg; + this->interval = interval * 1000; + this->mavlink = mavlink; + this->listener = listener; + this->subscriptions_n = subs_n; + this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); + + for (int i = 0; i < subs_n; i++) { + this->subscriptions[i] = listener->add_subscription(metas[i], sizes[i], this, interval); + } +} + +MavlinkStream::~MavlinkStream() +{ + free(subscriptions); +} + +/** + * Update mavlink stream, i.e. update subscriptions and send message if necessary + */ +int MavlinkStream::update(const hrt_abstime t) +{ + uint64_t dt = t - last_sent; + if (dt > 0 && dt >= interval) { + /* interval expired, update all subscriptions */ + for (unsigned int i = 0; i < subscriptions_n; i++) { + subscriptions[i]->update(t); + } + + /* format and send mavlink message */ + callback(this); + last_sent = t; + } +} |