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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
|
#include "LandDetector.h"
#include <unistd.h> //usleep
#include <drivers/drv_hrt.h>
LandDetector::LandDetector() :
_landDetectedPub(-1),
_landDetected({0, false}),
_taskShouldExit(false),
_taskIsRunning(false)
{
//ctor
}
LandDetector::~LandDetector()
{
_taskShouldExit = true;
close(_landDetectedPub);
}
void LandDetector::shutdown()
{
_taskShouldExit = true;
}
void LandDetector::start()
{
//Make sure this method has not already been called by another thread
if (isRunning()) {
return;
}
//Advertise the first land detected uORB
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = false;
_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
//Initialize land detection algorithm
initialize();
//Task is now running, keep doing so until shutdown() has been called
_taskIsRunning = true;
_taskShouldExit = false;
while (isRunning()) {
bool landDetected = update();
//Publish if land detection state has changed
if (_landDetected.landed != landDetected) {
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = landDetected;
/* publish the land detected broadcast */
orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected);
}
//Limit loop rate
usleep(1000000 / LAND_DETECTOR_UPDATE_RATE);
}
_taskIsRunning = false;
_exit(0);
}
bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
{
bool newData = false;
//Check if there is new data to grab
if (orb_check(handle, &newData) != OK) {
return false;
}
if (!newData) {
return false;
}
if (orb_copy(meta, handle, buffer) != OK) {
return false;
}
return true;
}
|
