fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 08:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

land_detector: big refactor, share update function

Browse Source 
This is a big refactor and general cleanup of the land detector. The
main functional change is to share the hysteresis lib across all land
detectors.
This commit is contained in:
Julian Oes
2016-07-30 12:26:56 +02:00
parent 9f928308c4
commit 50cac88e5b
9 changed files with 337 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.cpp
+89 -30
View File
@@ -31,32 +31,35 @@
*
****************************************************************************/
/**
/*
* @file LandDetector.cpp
* Land detection algorithm
*
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Morten Lysgaard <morten@lysgaard.no>
* @author Julian Oes <julian@oes.ch>
*/
#include "LandDetector.h"
#include <unistd.h> //usleep
#include <drivers/drv_hrt.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
namespace landdetection
#include "LandDetector.h"
namespace land_detector
{
LandDetector::LandDetector() :
_landDetectedPub(nullptr),
_landDetected{0, false, false},
_arming_time(0),
_freefall_hysteresis(false),
_landed_hysteresis(true),
_taskShouldExit(false),
_taskIsRunning(false),
_work{}
{
// ctor
// Use Trigger time when transitioning from in-air (false) to landed (true).
_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
}
LandDetector::~LandDetector()
@@ -67,60 +70,115 @@ LandDetector::~LandDetector()
int LandDetector::start()
{
_taskShouldExit = false;
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0);
return 0;
}
void LandDetector::shutdown()
void LandDetector::stop()
{
_taskShouldExit = true;
}
void
LandDetector::cycle_trampoline(void *arg)
LandDetector::_cycle_trampoline(void *arg)
{
LandDetector *dev = reinterpret_cast<LandDetector *>(arg);
dev->cycle();
dev->_cycle();
}
void LandDetector::cycle()
void LandDetector::_cycle()
{
if (!_taskIsRunning) {
// advertise the first land detected uORB
PX4_INFO("we are doing the init");
// Advertise the first land detected uORB.
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = false;
_landDetected.freefall = false;
// initialize land detection algorithm
initialize();
// Initialize uORB topics.
_initialize_topics();
// task is now running, keep doing so until shutdown() has been called
_check_params(true);
// Task is now running, keep doing so until we need to stop.
_taskIsRunning = true;
_taskShouldExit = false;
}
LandDetectionResult current_state = update();
bool landDetected = (current_state == LANDDETECTION_RES_LANDED);
bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL);
_check_params(false);
_update_topics();
_update_state();
bool landDetected = (_state == LandDetectionState::LANDED);
bool freefallDetected = (_state == LandDetectionState::FREEFALL);
// Only publish very first time or when the result has changed.
if ((_landDetectedPub == nullptr) ||
(_landDetected.landed != landDetected) ||
(_landDetected.freefall != freefallDetected)) {
if (_landDetectedPub == nullptr || _landDetected.landed != landDetected || _landDetected.freefall != freefallDetected) {
_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = (current_state == LANDDETECTION_RES_LANDED);
_landDetected.freefall = (current_state == LANDDETECTION_RES_FREEFALL);
_landDetected.landed = (_state == LandDetectionState::LANDED);
_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
int instance;
orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, &instance, ORB_PRIO_DEFAULT);
orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
&instance, ORB_PRIO_DEFAULT);
}
if (!_taskShouldExit) {
work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
// Schedule next cycle.
work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this,
USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ));
} else {
_taskIsRunning = false;
}
}
bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
void LandDetector::_check_params(const bool force)
{
bool updated;
parameter_update_s paramUpdate;
orb_check(_parameterSub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
}
if (updated || force) {
_update_params();
}
}
void LandDetector::_update_state()
{
bool landed = _get_landed_state();
_landed_hysteresis.set_state_and_update(landed);
_freefall_hysteresis.set_state_and_update(_get_freefall_state());
if (_freefall_hysteresis.get_state()) {
_state = LandDetectionState::FREEFALL;
} else if (_landed_hysteresis.get_state()) {
_state = LandDetectionState::LANDED;
} else {
_state = LandDetectionState::FLYING;
}
return;
}
bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer)
{
bool newData = false;
@@ -140,4 +198,5 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void
return true;
}
}
} // namespace land_detector