src/modules/mavlink/mavlink_orb_subscription.cpp


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

/*
 * mavlink_orb_subscription.cpp
 *
 *  Created on: 23.02.2014
 *      Author: ton
 */


#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <uORB/uORB.h>

#include "mavlink_orb_subscription.h"

MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
{
	_data = malloc(size);
	memset(_data, 0, size);
	_fd = orb_subscribe(_topic);
}

MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
	close(_fd);
	free(_data);
}

const struct orb_metadata *
MavlinkOrbSubscription::get_topic()
{
	return _topic;
}

void *
MavlinkOrbSubscription::get_data()
{
	return _data;
}

bool
MavlinkOrbSubscription::update(const hrt_abstime t)
{
	if (_last_check != t) {
		_last_check = t;
		bool updated;
		orb_check(_fd, &updated);
		if (updated) {
			orb_copy(_topic, _fd, _data);
			return true;
		}
	}
	return false;
}