yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Land detector: Run in work queue

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-10 16:02:05 +02:00
parent 129a4c3b53
commit 0318c1b330
3 changed files with 56 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
src/modules/land_detector/LandDetector.cpp
View File

@@ -50,59 +50,69 @@ LandDetector::LandDetector() :
_landDetected({0, false}),
_arming_time(0),
_taskShouldExit(false),
_taskIsRunning(false)
_taskIsRunning(false),
_work{}
{
// ctor
}
LandDetector::~LandDetector()
{
work_cancel(LPWORK, &_work);
_taskShouldExit = true;
}
int LandDetector::start()
{
/* schedule a cycle to start things */
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
return 0;
}
void LandDetector::shutdown()
{
_taskShouldExit = true;
}
void LandDetector::start()
void
LandDetector::cycle_trampoline(void *arg)
{
// make sure this method has not already been called by another thread
if (isRunning()) {
return;
LandDetector *dev = reinterpret_cast<LandDetector *>(arg);
dev->cycle();
}
void LandDetector::cycle()
{
if (!_taskIsRunning) {
// 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;
}
// advertise the first land detected uORB
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = false;
_landDetectedPub = (uintptr_t)orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
bool landDetected = update();
// initialize land detection algorithm
initialize();
// publish if land detection state has changed
if (_landDetected.landed != landDetected) {
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = landDetected;
// 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), (orb_advert_t)_landDetectedPub, &_landDetected);
}
// limit loop rate
usleep(1000000 / LAND_DETECTOR_UPDATE_RATE);
// publish the land detected broadcast
orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
}
_taskIsRunning = false;
_exit(0);
if (!_taskShouldExit) {
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
}
}
bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)