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

land detector uniform initialization cleanup

Browse Source
This commit is contained in:
Daniel Agar
2017-08-31 22:49:44 -04:00
parent cb8cc9a795
commit 6e402bd6f4
2 changed files with 29 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.cpp
+16 -30
View File
@@ -51,17 +51,6 @@ namespace land_detector
{
LandDetector::LandDetector() :
_landDetectedPub(nullptr),
_landDetected{0, false, false},
_parameterSub(0),
_state{},
_freefall_hysteresis(false),
_landed_hysteresis(true),
_maybe_landed_hysteresis(true),
_ground_contact_hysteresis(true),
_total_flight_time{0},
_takeoff_time{0},
_work{},
_cycle_perf(perf_alloc(PC_ELAPSED, "land_detector_cycle"))
{
}
@@ -96,6 +85,7 @@ void LandDetector::_cycle()
_landDetected.landed = false;
_landDetected.ground_contact = false;
_landDetected.maybe_landed = false;
_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
@@ -108,28 +98,24 @@ void LandDetector::_cycle()
}
_check_params(false);
_update_topics();
hrt_abstime now = hrt_absolute_time();
_update_state();
float alt_max_prev = _altitude_max;
_altitude_max = _get_max_altitude();
bool freefallDetected = (_state == LandDetectionState::FREEFALL);
bool landDetected = (_state == LandDetectionState::LANDED);
bool maybe_landedDetected = (_state == LandDetectionState::MAYBE_LANDED);
bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
const bool landDetected = (_state == LandDetectionState::LANDED);
const bool freefallDetected = (_state == LandDetectionState::FREEFALL);
const bool maybe_landedDetected = (_state == LandDetectionState::MAYBE_LANDED);
const bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
const float alt_max = _get_max_altitude();
// Only publish very first time or when the result has changed.
if ((_landDetectedPub == nullptr) ||
(_landDetected.freefall != freefallDetected) ||
(_landDetected.landed != landDetected) ||
(_landDetected.ground_contact != ground_contactDetected) ||
(_landDetected.freefall != freefallDetected) ||
(_landDetected.maybe_landed != maybe_landedDetected) ||
(fabsf(_landDetected.alt_max - alt_max_prev) > FLT_EPSILON)) {
(_landDetected.ground_contact != ground_contactDetected) ||
(fabsf(_landDetected.alt_max - alt_max) > FLT_EPSILON)) {
hrt_abstime now = hrt_absolute_time();
if (!landDetected && _landDetected.landed) {
// We did take off
@@ -146,11 +132,11 @@ void LandDetector::_cycle()
}
_landDetected.timestamp = hrt_absolute_time();
_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
_landDetected.landed = (_state == LandDetectionState::LANDED);
_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
_landDetected.maybe_landed = (_state == LandDetectionState::MAYBE_LANDED);
_landDetected.alt_max = _altitude_max;
_landDetected.landed = landDetected;
_landDetected.freefall = freefallDetected;
_landDetected.maybe_landed = maybe_landedDetected;
_landDetected.ground_contact = ground_contactDetected;
_landDetected.alt_max = alt_max;
int instance;
orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,