diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 8811da6d34..b318df490d 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -61,18 +61,7 @@ PrecLand::PrecLand(Navigator *navigator) : MissionBlock(navigator), - ModuleParams(navigator), - _targetPoseSub(0), - _target_pose_valid(false), - _state_start_time(0), - _search_cnt(0), - _approach_alt(0) - -{ -} - -void -PrecLand::on_inactive() + ModuleParams(navigator) { } @@ -127,7 +116,7 @@ PrecLand::on_active() _target_pose_valid = true; } - if (hrt_absolute_time() - _target_pose.timestamp > (uint64_t)(_param_timeout.get()*SEC2USEC)) { + if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e-6f) > _param_timeout.get()) { _target_pose_valid = false; } @@ -278,7 +267,6 @@ PrecLand::run_state_horizontal_approach() pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _navigator->set_position_setpoint_triplet_updated(); - } void @@ -316,7 +304,6 @@ PrecLand::run_state_descend_above_target() pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; _navigator->set_position_setpoint_triplet_updated(); - } void diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index 819c7126ce..0d9177e5d3 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -67,14 +67,10 @@ class PrecLand : public MissionBlock, public ModuleParams { public: PrecLand(Navigator *navigator); + ~PrecLand() override = default; - ~PrecLand() = default; - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); + void on_activation() override; + void on_active() override; void set_mode(PrecLandMode mode) { _mode = mode; }; @@ -103,22 +99,26 @@ private: void slewrate(float &sp_x, float &sp_y); landing_target_pose_s _target_pose{}; /**< precision landing target position */ - int _targetPoseSub; - bool _target_pose_valid; /**< wether we have received a landing target position message */ + + int _targetPoseSub{-1}; + bool _target_pose_valid{false}; /**< wether we have received a landing target position message */ + struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */ - uint64_t _state_start_time; /**< time when we entered current state */ - uint64_t _last_slewrate_time; /**< time when we last limited setpoint changes */ - uint64_t _target_acquired_time; /**< time when we first saw the landing target during search */ - uint64_t _point_reached_time; /**< time when we reached a setpoint */ - int _search_cnt; /**< counter of how many times we had to search for the landing target */ - float _approach_alt; /**< altitude at which to stay during horizontal approach */ + + uint64_t _state_start_time{0}; /**< time when we entered current state */ + uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */ + uint64_t _target_acquired_time{0}; /**< time when we first saw the landing target during search */ + uint64_t _point_reached_time{0}; /**< time when we reached a setpoint */ + + int _search_cnt{0}; /**< counter of how many times we had to search for the landing target */ + float _approach_alt{0.0f}; /**< altitude at which to stay during horizontal approach */ matrix::Vector2f _sp_pev; matrix::Vector2f _sp_pev_prev; - PrecLandState _state; + PrecLandState _state{PrecLandState::Start}; - PrecLandMode _mode; + PrecLandMode _mode{PrecLandMode::Opportunistic}; DEFINE_PARAMETERS( (ParamFloat) _param_timeout,