diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 462dccd9a8..b8b20d8ab0 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -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, diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index c65980ac5b..1604de4ecf 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -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; };