From 2a765ee8090d2150ef3d21a338f0df34a9294c8c Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 15 Mar 2023 14:30:35 -0800 Subject: [PATCH] rewrote the precision landing state machine and logic --- .../airframes/1011_gazebo-classic_iris_irlock | 2 + .../flight_mode_manager/FlightModeManager.cpp | 15 +- .../tasks/Auto/FlightTaskAuto.cpp | 2 + .../FlightTaskAutoPrecisionLanding.cpp | 464 +++++++----------- .../FlightTaskAutoPrecisionLanding.hpp | 81 +-- 5 files changed, 229 insertions(+), 335 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock index 2f064e2c5f..11c5ae6ed6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock @@ -33,6 +33,8 @@ param set-default PWM_MAIN_FUNC4 104 param set-default LTEST_MODE 1 param set-default PLD_HACC_RAD 0.1 param set-default RTL_PLD_MD 2 +param set-default MPC_LAND_SPEED 0.2 +param set-default PLD_SRCH_TOUT 30 # Start up Landing Target Estimator module landing_target_estimator start diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 31d4bdd880..ba0e8327a5 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -140,16 +140,23 @@ void FlightModeManager::start_flight_task() bool task_failure = false; bool should_disable_task = true; - const bool land_should_be_precland = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL - || - _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) - && _param_rtl_pld_md.get() > 0; + // land/rtl mode is precland + const bool land_should_be_precland = + (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL || + _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) + && _param_rtl_pld_md.get() > 0; + + // Mission item precland const bool precland_mission_item_active = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION && _navigator_mission_item_sub.get().nav_sub_cmd == navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND; + + // When issuing the auto:precland mode const bool precland_flight_mode = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; + // PX4_INFO("start_flight_task: %d %d %d", land_should_be_precland, precland_mission_item_active, precland_flight_mode); + // Do not run any flight task for VTOLs in fixed-wing mode if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { switchTask(FlightTaskIndex::None); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index a5d0c00aab..3e5444a1a5 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -215,6 +215,7 @@ void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s) void FlightTaskAuto::_prepareLandSetpoints() { + // PX4_INFO("FlightTaskAuto::_prepareLandSetpoints"); _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint // Slow down automatic descend close to ground @@ -323,6 +324,7 @@ bool FlightTaskAuto::_evaluateTriplets() // Check if triplet is valid. There must be at least a valid altitude. if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) { + PX4_INFO("forcing setpoint type to loiter"); // Best we can do is to just set all waypoints to current state _prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position; _type = WaypointType::loiter; diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 9e4058fccb..14fcc98a62 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -43,16 +43,55 @@ #include #include +const char* STATE_STRINGS[] = {"Idle", "Start", "HorizontalApproach", "DescendAboveTarget", "Search", "NormalLand", "Finished"}; static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint) { + // PX4_INFO("FlightTaskAutoPrecisionLanding::activate"); bool ret = FlightTask::activate(last_setpoint); - _search_cnt = 0; + _search_count = 0; _last_slewrate_time = 0; + _landing_target_valid = false; + _state = PrecLandState::Idle; + _fix_this_activate_update_loop = true; - try_switch_to_state_auto_rtl(); + + _sub_vehicle_status.update(); + + + + + + return ret; +} + +bool FlightTaskAutoPrecisionLanding::updateInitialize() +{ + // PX4_INFO("updateInitialize"); + bool ret = FlightTask::updateInitialize(); + + _sub_home_position.update(); + _sub_vehicle_status.update(); + + uint8_t nav_state = _sub_vehicle_status.get().nav_state; + + // Mission precision land + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + _type = WaypointType::land; + + // Auto RTL + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _type = WaypointType::loiter; + + // Auto Land + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + _type = WaypointType::land; + } + + // require valid position + ret = ret && _position.isAllFinite() && _velocity.isAllFinite(); return ret; } @@ -61,37 +100,37 @@ bool FlightTaskAutoPrecisionLanding::update() { bool ret = FlightTaskAuto::update(); - // Fetch uorb if (_landing_target_pose_sub.updated()) { _landing_target_pose_sub.copy(&_landing_target_pose); } // target pose can become invalid when the message timed out - _landing_target_pose_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); + _landing_target_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); switch (_state) { - case PrecLandState::AutoRTL: - run_state_auto_rtl(); + + case PrecLandState::Idle: + run_state_idle(); break; - case PrecLandState::MoveAboveTarget: - run_state_move_above_target(); + case PrecLandState::Start: + run_state_start(); + break; + + case PrecLandState::HorizontalApproach: + run_state_horizontal_approach(); break; case PrecLandState::DescendAboveTarget: run_state_descend_above_target(); break; - case PrecLandState::TouchingDown: - run_state_touching_down(); - break; - case PrecLandState::Search: run_state_search(); break; - case PrecLandState::Fallback: - run_state_fallback(); + case PrecLandState::NormalLand: + run_state_normal_land(); break; default: @@ -105,314 +144,153 @@ bool FlightTaskAutoPrecisionLanding::update() precision_landing_status.precland_state = (uint8_t) _state; _precision_landing_status_pub.publish(precision_landing_status); - _constraints.want_takeoff = _checkTakeoff(); + // What is this? Wtf? + // _constraints.want_takeoff = _checkTakeoff(); return ret; } -void -FlightTaskAutoPrecisionLanding::run_state_auto_rtl() +void FlightTaskAutoPrecisionLanding::switch_state(PrecLandState state) { - // In this state simply track the navigator setpoint triplet - // This ensures that the behaviour for precision landing during RTL / LAND - // is the same as without precision landing. - // This flight task will generate independent setpoints once the vehicle - // is in the vertical landing phase. - _position_setpoint = _target; + state_on_exit(_state); + _state = state; + state_on_enter(_state); + PX4_INFO("Switching to %s", STATE_STRINGS[static_cast(state)]); +} - if (try_switch_to_state_move_above_target()) { - // If target visible and go to horizontal approach directly - return; - - } else if ((PrecLandMode)_param_rtl_pld_md.get() == PrecLandMode::Opportunistic) { - // could not see the target immediately, so just fall back to normal landing - try_switch_to_state_fallback(); - - } else if (_type == WaypointType::land) { - // Navigator already entered land stage. Take over with precision landing - // This is where the vehicle starts to behave differently than in regular land! - try_switch_to_state_search(); +void FlightTaskAutoPrecisionLanding::state_on_enter(PrecLandState state) +{ + switch (state) { + case PrecLandState::Search: + _search_count++; + _search_start_time = hrt_absolute_time(); + break; + case PrecLandState::HorizontalApproach: + _horizontal_approach_alt = _position(2); + break; + default: + break; } } -void -FlightTaskAutoPrecisionLanding::run_state_move_above_target() +void FlightTaskAutoPrecisionLanding::state_on_exit(PrecLandState state) { + // TODO: + (void)state; +} + +void FlightTaskAutoPrecisionLanding::run_state_idle() +{ + if (_fix_this_activate_update_loop) { + _fix_this_activate_update_loop = false; + return; + } + + // PX4_INFO("run_state_idle: waypoint type: %d", (int)_type); + + if (_type == WaypointType::land) { + switch_state(PrecLandState::Start); + } +} + +void FlightTaskAutoPrecisionLanding::run_state_start() +{ + // Initialize our position setpoint to the current position + _position_setpoint = _target = _position; + + + if (_landing_target_valid) { + switch_state(PrecLandState::HorizontalApproach); + + } else { + + PX4_INFO("Target not seen"); + + // Check if opportunistic mode is enabled + static constexpr int32_t REQUIRED = 2; + if (_param_rtl_pld_md.get() == REQUIRED) { + switch_state(PrecLandState::Search); + + } else { + switch_state(PrecLandState::NormalLand); + } + } +} + +void FlightTaskAutoPrecisionLanding::run_state_normal_land() +{ + // Do nothing +} + +void FlightTaskAutoPrecisionLanding::run_state_search() +{ + _position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get(); + // _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; + + // If target is seen run horizontal approach + if (_landing_target_valid) { + switch_state(PrecLandState::HorizontalApproach); + return; + } + + // If we exceed PLD_SRCH_TOUT fallback to normal land + if ((hrt_elapsed_time(&_search_start_time) / 1e6f) >= _param_pld_srch_tout.get()) { + PX4_INFO("Search timed out"); + switch_state(PrecLandState::NormalLand); + } + + // Check if we have exceeded the maximum number of search attempts + if (_search_count > _param_pld_max_srch.get()) { + PX4_INFO("Maximum search attempts exceeded (%d)", _search_count); + switch_state(PrecLandState::NormalLand); + } +} + +void FlightTaskAutoPrecisionLanding::run_state_horizontal_approach() +{ + if (!_landing_target_valid) { + switch_state(PrecLandState::Search); + return; + } + float x = _landing_target_pose.x_abs; float y = _landing_target_pose.y_abs; slewrate(x, y); // TODO: Replace this with PX4's AlphaFilter - // Fly to target XY position, but keep navigator's altitude setpoint + // Fly to target XY position, maintain current altitude _position_setpoint(0) = x; _position_setpoint(1) = y; - _position_setpoint(2) = _target(2); + _position_setpoint(2) = _horizontal_approach_alt; - _velocity_setpoint(0) = _velocity_setpoint(1) = NAN; + // Check if it's time to descend + Vector2f target_position_vector = Vector2f(_landing_target_pose.x_abs,_landing_target_pose.y_abs); + Vector2f position_delta_vector = Vector2f(target_position_vector - _position.xy()); - // If altitude setpoint is NAN, we stay at the current altitude - if (!PX4_ISFINITE(_position_setpoint(2))) { - _velocity_setpoint(2) = 0.0f; + if (position_delta_vector.norm() <= _param_pld_hacc_rad.get() ) { + switch_state(PrecLandState::DescendAboveTarget); } +} - // check if target visible, if not go to start - if (!check_state_conditions(PrecLandState::MoveAboveTarget)) { - PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state); - - // Stay at current position for searching for the landing target - // TODO: This is not going to work after conversion to flight task, because - // the position_setpoint will be overwritten at the next iteration! - // Solution: An additional wait state - _position_setpoint = _position; - _velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN; - - if (!try_switch_to_state_auto_rtl()) { - try_switch_to_state_fallback(); - } - +void FlightTaskAutoPrecisionLanding::run_state_descend_above_target() +{ + if (!_landing_target_valid) { + switch_state(PrecLandState::Search); return; } - if (check_state_conditions(PrecLandState::DescendAboveTarget)) { - if (!_point_reached_time) { - _point_reached_time = hrt_absolute_time(); - } - - if (hrt_absolute_time() - _point_reached_time > 2000000) { - // if close enough for descent above target go to descend above target - if (try_switch_to_state_descend_above_target()) { - - return; - } - } - - } -} - -void -FlightTaskAutoPrecisionLanding::run_state_descend_above_target() -{ - // Overwrite Auto setpoints in order to descend above target + // Let's assume the FlightTask::update() function is preparing our land setpoints. _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(); - - // check if target visible - if (!check_state_conditions(PrecLandState::DescendAboveTarget)) { - if (!try_switch_to_state_touching_down()) { - PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state); - - // Stay at current position for searching for the target - _position_setpoint = _position; - - if (!try_switch_to_state_auto_rtl()) { - try_switch_to_state_fallback(); - } - } - - return; + // Check if we're within our final approach altitude + if ((_landing_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get()) { + switch_state(PrecLandState::Finished); } } -void -FlightTaskAutoPrecisionLanding::run_state_touching_down() +void FlightTaskAutoPrecisionLanding::run_state_finished() { - // Overwrite Auto setpoints in order to land at target's last known location - _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(); -} - -void -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 - if (check_state_conditions(PrecLandState::MoveAboveTarget)) { - if (!_target_acquired_time) { - // target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly - _target_acquired_time = hrt_absolute_time(); - _position_setpoint = _position; - _position_setpoint(2) += 1.0f; - } - - } - - // stay at that height for a second to allow the vehicle to settle - if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) { - // try to switch to horizontal approach - if (try_switch_to_state_move_above_target()) { - return; - } - - } 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"); - - try_switch_to_state_fallback(); - } -} - -void -FlightTaskAutoPrecisionLanding::run_state_fallback() -{ - // Try switching to horizontal approach. This works if the - // target meanwhile became visible - try_switch_to_state_move_above_target(); - - // Otherwise there is nothing to do, except for listening to navigator - _position_setpoint = _target; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_auto_rtl() -{ - if (check_state_conditions(PrecLandState::AutoRTL)) { - _search_cnt++; - - _point_reached_time = 0; - - _state = PrecLandState::AutoRTL; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_move_above_target() -{ - if (check_state_conditions(PrecLandState::MoveAboveTarget)) { - print_state_switch_message("horizontal approach"); - - _point_reached_time = 0; - - _state = PrecLandState::MoveAboveTarget; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_descend_above_target() -{ - if (check_state_conditions(PrecLandState::DescendAboveTarget)) { - print_state_switch_message("descend"); - _state = PrecLandState::DescendAboveTarget; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -bool -FlightTaskAutoPrecisionLanding::try_switch_to_state_touching_down() -{ - if (check_state_conditions(PrecLandState::TouchingDown)) { - print_state_switch_message("final approach"); - _state = PrecLandState::TouchingDown; - _state_start_time = hrt_absolute_time(); - return true; - } - - return false; -} - -void -FlightTaskAutoPrecisionLanding::try_switch_to_state_search() -{ - PX4_INFO("Climbing to search altitude"); - - _target_acquired_time = 0; - - _state = PrecLandState::Search; - _state_start_time = hrt_absolute_time(); -} - -void -FlightTaskAutoPrecisionLanding::try_switch_to_state_fallback() -{ - print_state_switch_message("fallback"); - - _state = PrecLandState::Fallback; - _state_start_time = hrt_absolute_time(); -} - -void FlightTaskAutoPrecisionLanding::print_state_switch_message(const char *state_name) -{ - PX4_INFO("Precland: switching to %s", state_name); -} - -bool FlightTaskAutoPrecisionLanding::check_state_conditions(PrecLandState state) -{ - switch (state) { - case PrecLandState::AutoRTL: - return _search_cnt <= _param_pld_max_srch.get(); - - case PrecLandState::MoveAboveTarget: - - // 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 (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; - - } else { - // We've seen the target sometime during horizontal approach. - // Even if we don't see it as we're moving towards it, continue approaching last known location - return true; - } - } - - // If we're trying to switch to this state, the target needs to be visible - return _landing_target_pose_valid && _landing_target_pose.abs_pos_valid; - - case PrecLandState::DescendAboveTarget: - - // if we're already in this state, only leave it if target becomes unusable, don't care about horizontal offset to target - 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() - _landing_target_pose.timestamp < 500000; // 0.5s // TODO: Magic number! - - } else { - 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 _landing_target_pose.abs_pos_valid && hor_acc_radius_check(); - } - - case PrecLandState::TouchingDown: - 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; - - case PrecLandState::Fallback: - return true; - - default: - return false; - } + // Nothing to do. } bool FlightTaskAutoPrecisionLanding::hor_acc_radius_check() @@ -433,7 +311,7 @@ void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y) return; } - dt /= SEC2USEC; + dt /= 1000000; if (_last_slewrate_time == 0) { // running the first time since switching to precland diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp index 73d4af50ba..69e36a2f91 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -52,18 +52,14 @@ #include #include -#define SEC2USEC 1000000.0f // TODO: Get the correct define from some header -#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state -#define ACCEPTANCE_RADIUS 0.20f // Horizontal acceptance radius for the navigation to the landing target -// TODO: Get ACCEPTANCE_RADIUS from NAV_ACC_RAD - enum class PrecLandState { - AutoRTL, // Starting state - Search, // Search for landing target - MoveAboveTarget, // Positioning over landing target while maintaining altitude - DescendAboveTarget, // Stay over landing target while descending - TouchingDown, // Final landing approach, even without landing target - Fallback // Fallback landing method + Idle, + Start, + HorizontalApproach, + DescendAboveTarget, + Search, + NormalLand, + Finished, }; enum class PrecLandMode { @@ -81,49 +77,58 @@ public: bool update() override; + bool updateInitialize() override; + private: - // run the control loop for each state - void run_state_auto_rtl(); - void run_state_search(); - void run_state_move_above_target(); + + // Jake: + void run_state_idle(); + void run_state_start(); + void run_state_horizontal_approach(); void run_state_descend_above_target(); - void run_state_touching_down(); - void run_state_fallback(); + void run_state_search(); + void run_state_normal_land(); + void run_state_finished(); - // attempt to switch to a different state. Returns true if state change was successful, false otherwise - bool try_switch_to_state_auto_rtl(); - void try_switch_to_state_search(); - bool try_switch_to_state_move_above_target(); - bool try_switch_to_state_descend_above_target(); - bool try_switch_to_state_touching_down(); - void try_switch_to_state_fallback(); + void switch_state(PrecLandState state); + void state_on_enter(PrecLandState state); + void state_on_exit(PrecLandState state); - void print_state_switch_message(const char *state_name); - - // check if a given state could be changed into. Return true if possible to transition to state, false otherwise - 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)}; uORB::PublicationMulti _precision_landing_status_pub{ORB_ID(precision_landing_status)}; - bool _landing_target_pose_valid{false}; /**< whether we have received a landing target position message */ + landing_target_pose_s _landing_target_pose {}; /**< precision landing target position */ - 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 */ + float _horizontal_approach_alt {}; - int _search_cnt{0}; /**< counter of how many times we had to search for the landing target */ + // FIX THIS + // INFO [FlightTaskAutoPrecisionLanding] Switching to Start + // INFO [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate + // INFO [FlightTaskAutoPrecisionLanding] run_state_idle: waypoint type: 4 + // INFO [FlightTaskAutoPrecisionLanding] Switching to Start + // INFO [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate + // INFO [FlightTaskAutoPrecisionLanding] run_state_idle: waypoint type: 4 + // INFO [FlightTaskAutoPrecisionLanding] Switching to Start + // INFO [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate + // INFO [FlightTaskAutoPrecisionLanding] run_state_idle: waypoint type: 4 + // INFO [FlightTaskAutoPrecisionLanding] Switching to Start + bool _fix_this_activate_update_loop {true}; + + + bool _landing_target_valid {false}; /**< whether we have received a landing target position message */ + + hrt_abstime _search_start_time {0}; + + uint64_t _last_slewrate_time {0}; /**< time when we last limited setpoint changes */ + int _search_count {0}; matrix::Vector2f _sp_pev; matrix::Vector2f _sp_pev_prev; - PrecLandState _state{PrecLandState::AutoRTL}; + PrecLandState _state{PrecLandState::Idle}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_land_speed, ///< velocity for controlled descend