From 5bbf12737b9ccace4925f2e1e424710d7c07be7a Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Mon, 13 Mar 2023 16:17:25 +0100 Subject: [PATCH] rename variable --- .../FlightTaskAutoPrecisionLanding.cpp | 47 ++++++++++--------- .../FlightTaskAutoPrecisionLanding.hpp | 6 +-- 2 files changed, 28 insertions(+), 25 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 5a0ef2d465..fb06bd8833 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -62,12 +62,12 @@ bool FlightTaskAutoPrecisionLanding::update() bool ret = FlightTaskAuto::update(); // Fetch uorb - if (_target_pose_sub.updated()) { - _target_pose_sub.copy(&_target_pose); + if (_landing_target_pose_sub.updated()) { + _landing_target_pose_sub.copy(&_landing_target_pose); } // target pose can become invalid when the message timed out - _target_pose_valid = (hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); + _landing_target_pose_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); switch (_state) { case PrecLandState::AutoRTL: @@ -138,8 +138,8 @@ FlightTaskAutoPrecisionLanding::run_state_auto_rtl() void FlightTaskAutoPrecisionLanding::run_state_move_above_target() { - float x = _target_pose.x_abs; - float y = _target_pose.y_abs; + float x = _landing_target_pose.x_abs; + float y = _landing_target_pose.y_abs; slewrate(x, y); // Fly to target XY position, but keep navigator's altitude setpoint @@ -192,9 +192,10 @@ void FlightTaskAutoPrecisionLanding::run_state_descend_above_target() { // Overwrite Auto setpoints in order to descend above target - _position_setpoint(0) = _target_pose.x_abs; - _position_setpoint(1) = _target_pose.y_abs; + _position_setpoint(0) = _landing_target_pose.x_abs; + _position_setpoint(1) = _landing_target_pose.y_abs; _position_setpoint(2) = NAN; + _velocity_setpoint(0) = 0; _velocity_setpoint(1) = 0; _velocity_setpoint(2) = _param_mpc_land_speed.get(); @@ -220,9 +221,10 @@ void FlightTaskAutoPrecisionLanding::run_state_touching_down() { // Overwrite Auto setpoints in order to land at target's last known location - _position_setpoint(0) = _target_pose.x_abs; - _position_setpoint(1) = _target_pose.y_abs; + _position_setpoint(0) = _landing_target_pose.x_abs; + _position_setpoint(1) = _landing_target_pose.y_abs; _position_setpoint(2) = NAN; + _velocity_setpoint(0) = 0; _velocity_setpoint(1) = 0; _velocity_setpoint(2) = _param_mpc_land_speed.get(); @@ -234,6 +236,7 @@ FlightTaskAutoPrecisionLanding::run_state_search() // Overwrite Auto setpoints in order to hover at search altitude _position_setpoint = _target; _position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get(); + _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; // check if we can see the target @@ -253,10 +256,9 @@ FlightTaskAutoPrecisionLanding::run_state_search() if (switch_to_state_move_above_target()) { return; } - } - - // check if search timed out and go to fallback + } else if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) { + // Search timed out and go to fallback PX4_WARN("Search timed out"); switch_to_state_fallback(); @@ -269,6 +271,7 @@ FlightTaskAutoPrecisionLanding::run_state_fallback() // Try switching to horizontal approach. This works if the // target meanwhile became visible switch_to_state_move_above_target(); + // Otherwise there is nothing to do, except for listening to navigator _position_setpoint = _target; } @@ -366,9 +369,9 @@ 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(_target_pose.x_abs, _target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get()) { + if (Vector2f(Vector2f(_landing_target_pose.x_abs, _landing_target_pose.y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get()) { // we've reached the position where we last saw the target. If we don't see it now, we need to do something - return _target_pose_valid && _target_pose.abs_pos_valid; + return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; } else { // We've seen the target sometime during horizontal approach. @@ -378,7 +381,7 @@ bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) } // If we're trying to switch to this state, the target needs to be visible - return _target_pose_valid && _target_pose.abs_pos_valid; + return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; case PrecLandState::DescendAboveTarget: @@ -386,22 +389,22 @@ bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) if (_state == PrecLandState::DescendAboveTarget) { // if we're close to the ground, we're more critical of target timeouts so we quickly go into descend if (check_state_conditions(PrecLandState::TouchingDown)) { - return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s // TODO: Magic number! + return hrt_absolute_time() - _landing_target_pose.timestamp < 500000; // 0.5s // TODO: Magic number! } else { - return _target_pose_valid && _target_pose.abs_pos_valid; + return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; } } else { // if not already in this state, need to be above target to enter it - return _target_pose.abs_pos_valid - && fabsf(_target_pose.x_abs - _position(0)) < _param_pld_hacc_rad.get() - && fabsf(_target_pose.y_abs - _position(1)) < _param_pld_hacc_rad.get(); + 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(); } case PrecLandState::TouchingDown: - return _target_pose_valid && _target_pose.abs_pos_valid - && (_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get(); + return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid + && (_landing_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get(); case PrecLandState::Search: return true; diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp index 10d3d103df..9b1bc3b629 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -104,12 +104,12 @@ private: bool check_state_conditions(PrecLandState state); void slewrate(float &sp_x, float &sp_y); - landing_target_pose_s _target_pose{}; /**< precision landing target position */ + landing_target_pose_s _landing_target_pose{}; /**< precision landing target position */ - uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)}; + uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; uORB::PublicationMulti _precision_landing_status_pub{ORB_ID(precision_landing_status)}; - bool _target_pose_valid{false}; /**< whether we have received a landing target position message */ + bool _landing_target_pose_valid{false}; /**< whether we have received a landing target position message */ 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 */