diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-07-23 11:03:37 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-07-23 11:03:37 +0200 |
commit | 344a34bb725a65769a227157f41363fd5d180a14 (patch) | |
tree | b53b41b637567db4ba5f29c5ea0e5738686c99c5 /src/modules | |
parent | 4722b609ccfa35c438c1ef74caba2793c9e04225 (diff) | |
download | px4-firmware-344a34bb725a65769a227157f41363fd5d180a14.tar.gz px4-firmware-344a34bb725a65769a227157f41363fd5d180a14.tar.bz2 px4-firmware-344a34bb725a65769a227157f41363fd5d180a14.zip |
mavlink: MavlinkStream API updated
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mavlink/mavlink_stream.cpp | 5 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_stream.h | 13 |
2 files changed, 13 insertions, 5 deletions
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 79dc23cc6..da3a55172 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,11 +71,12 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + unsigned int interval = _interval * _mavlink->get_rate_mult(); - if (dt > 0 && dt >= _interval) { + if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = (t / _interval) * _interval; + _last_sent = t; return 0; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 7d3afbbb5..646931c07 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -46,8 +46,6 @@ class Mavlink; class MavlinkStream; -#include "mavlink_main.h" - class MavlinkStream { @@ -70,7 +68,6 @@ public: * @return the inveral in microseconds (us) between messages */ unsigned get_interval() { return _interval; } - void set_channel(mavlink_channel_t channel); /** * @return 0 if updated / sent, -1 if unchanged @@ -82,6 +79,16 @@ public: virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + protected: Mavlink * _mavlink; unsigned int _interval; |