diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index ac2b6d7e07..9e4058fccb 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -369,8 +369,7 @@ bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) // if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore if (_state == PrecLandState::MoveAboveTarget) { - if (Vector2f(Vector2f(_landing_target_pose.x_abs, - _landing_target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get()) { + if (hor_acc_radius_check()) { // we've reached the position where we last saw the target. If we don't see it now, we need to do something return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; @@ -398,9 +397,7 @@ bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) } else { // if not already in this state, need to be above target to enter it - return _landing_target_pose.abs_pos_valid - && fabsf(_landing_target_pose.x_abs - _position(0)) < _param_pld_hacc_rad.get() - && fabsf(_landing_target_pose.y_abs - _position(1)) < _param_pld_hacc_rad.get(); + return _landing_target_pose.abs_pos_valid && hor_acc_radius_check(); } case PrecLandState::TouchingDown: @@ -418,6 +415,12 @@ bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) } } +bool FlightTaskAutoPrecisionLanding::hor_acc_radius_check() +{ + return Vector2f(Vector2f(_landing_target_pose.x_abs, + _landing_target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get(); +} + void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y) { matrix::Vector2f sp_curr(sp_x, sp_y); diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp index 82cd0909c2..73d4af50ba 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -104,6 +104,8 @@ private: bool check_state_conditions(PrecLandState state); void slewrate(float &sp_x, float &sp_y); + bool hor_acc_radius_check(); + landing_target_pose_s _landing_target_pose{}; /**< precision landing target position */ uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};