diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_orb_subscription.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_orb_subscription.cpp | 57 |
1 files changed, 31 insertions, 26 deletions
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d432edd2b..734f0903a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -47,53 +47,58 @@ #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : - _fd(orb_subscribe(_topic)), - _published(false), + next(nullptr), _topic(topic), - _last_check(0), - next(nullptr) + _fd(orb_subscribe(_topic)), + _published(false) { - _data = malloc(topic->o_size); - memset(_data, 0, topic->o_size); } MavlinkOrbSubscription::~MavlinkOrbSubscription() { close(_fd); - free(_data); } -const orb_id_t -MavlinkOrbSubscription::get_topic() +orb_id_t +MavlinkOrbSubscription::get_topic() const { return _topic; } -void * -MavlinkOrbSubscription::get_data() -{ - return _data; -} - bool -MavlinkOrbSubscription::update(const hrt_abstime t) +MavlinkOrbSubscription::update(uint64_t *time, void* data) { - if (_last_check == t) { - /* already checked right now, return result of the check */ - return _updated; + // TODO this is NOT atomic operation, we can get data newer than time + // if topic was published between orb_stat and orb_copy calls. - } else { - _last_check = t; - orb_check(_fd, &_updated); + uint64_t time_topic; + if (orb_stat(_fd, &time_topic)) { + /* error getting last topic publication time */ + time_topic = 0; + } - if (_updated) { - orb_copy(_topic, _fd, _data); - _published = true; + if (orb_copy(_topic, _fd, data)) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + return false; + + } else { + /* data copied successfully */ + _published = true; + if (time_topic != *time) { + *time = time_topic; return true; + + } else { + return false; } } +} - return false; +bool +MavlinkOrbSubscription::update(void* data) +{ + return !orb_copy(_topic, _fd, data); } bool |