fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 18:08:53 +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,
src/modules/land_detector/LandDetector.h
+13 -16
View File
@@ -106,7 +106,6 @@ protected:
*/
virtual void _update_topics() = 0;
/**
* Update parameters.
*/
@@ -147,19 +146,17 @@ protected:
/** Run main land detector loop at this rate in Hz. */
static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
orb_advert_t _landDetectedPub;
struct vehicle_land_detected_s _landDetected;
orb_advert_t _landDetectedPub{nullptr};
vehicle_land_detected_s _landDetected{};
int _parameterSub;
int _parameterSub{-1};
LandDetectionState _state;
LandDetectionState _state{LandDetectionState::LANDED};
systemlib::Hysteresis _freefall_hysteresis;
systemlib::Hysteresis _landed_hysteresis;
systemlib::Hysteresis _maybe_landed_hysteresis;
systemlib::Hysteresis _ground_contact_hysteresis;
float _altitude_max;
systemlib::Hysteresis _freefall_hysteresis{false};
systemlib::Hysteresis _landed_hysteresis{true};
systemlib::Hysteresis _maybe_landed_hysteresis{true};
systemlib::Hysteresis _ground_contact_hysteresis{true};
private:
static void _cycle_trampoline(void *arg);
@@ -170,12 +167,12 @@ private:
void _update_state();
param_t _p_total_flight_time_high;
param_t _p_total_flight_time_low;
uint64_t _total_flight_time; ///< in microseconds
hrt_abstime _takeoff_time;
param_t _p_total_flight_time_high{PARAM_INVALID};
param_t _p_total_flight_time_low{PARAM_INVALID};
uint64_t _total_flight_time{0}; ///< in microseconds
hrt_abstime _takeoff_time{0};
struct work_s _work;
struct work_s _work {};
perf_counter_t _cycle_perf;
};