fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 02:03: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/FixedwingLandDetector.cpp
+23 -60
View File
@@ -31,22 +31,23 @@
*
****************************************************************************/
/**
/*
* @file FixedwingLandDetector.cpp
* Land detection algorithm for fixedwings
*
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
* @author Julian Oes <julian@oes.ch>
*/
#include "FixedwingLandDetector.h"
#include <px4_config.h>
#include <px4_defines.h>
#include <cmath>
#include <drivers/drv_hrt.h>
namespace landdetection
#include "FixedwingLandDetector.h"
namespace land_detector
{
FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
@@ -58,12 +59,10 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_controlState{},
_arming{},
_airspeed{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_accel_horz_lp(0.0f),
_landDetectTrigger(0)
_accel_horz_lp(0.0f)
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
@@ -71,54 +70,41 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
}
void FixedwingLandDetector::initialize()
void FixedwingLandDetector::_initialize_topics()
{
_controlStateSub = orb_subscribe(ORB_ID(control_state));
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
updateParameterCache(true);
}
void FixedwingLandDetector::updateSubscriptions()
void FixedwingLandDetector::_update_topics()
{
orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
_orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
}
LandDetectionResult FixedwingLandDetector::update()
void FixedwingLandDetector::_update_params()
{
// First poll for new data from our subscriptions
updateSubscriptions();
if (get_freefall_state()) {
_state = LANDDETECTION_RES_FREEFALL;
} else if (get_landed_state()) {
_state = LANDDETECTION_RES_LANDED;
} else {
_state = LANDDETECTION_RES_FLYING;
}
return _state;
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
}
bool FixedwingLandDetector::get_freefall_state()
bool FixedwingLandDetector::_get_freefall_state()
{
//TODO
// TODO
return false;
}
bool FixedwingLandDetector::get_landed_state()
bool FixedwingLandDetector::_get_landed_state()
{
// only trigger flight conditions if we are armed
if (!_arming.armed) {
return true;
}
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
@@ -147,14 +133,10 @@ bool FixedwingLandDetector::get_landed_state()
&& _airspeed_filtered < _params.maxAirSpeed
&& _accel_horz_lp < _params.maxIntVelocity) {
// these conditions need to be stable for a period of time before we trust them
if (now > _landDetectTrigger) {
landDetected = true;
}
landDetected = true;
} else {
// reset land detect trigger
_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
landDetected = false;
}
} else {
@@ -165,23 +147,4 @@ bool FixedwingLandDetector::get_landed_state()
return landDetected;
}
void FixedwingLandDetector::updateParameterCache(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) {
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
}
}
}
} // namespace land_detector