From 7bfcbd786526209125aecbb54bda14c1f628cfba Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 16 Mar 2023 11:56:07 -0800 Subject: [PATCH] added back inheritance from FlightTaskAuto, fixed logic in multiple places, use Navigator Land submode for auto_precision_land. Last remaining issues is Pause during precision land will place vehicle back at the XY where the Land was initiated. --- .../flight_mode_manager/FlightModeManager.cpp | 1 + .../tasks/Auto/FlightTaskAuto.cpp | 5 +- .../FlightTaskAutoPrecisionLanding.cpp | 86 +++++++++---------- .../FlightTaskAutoPrecisionLanding.hpp | 4 +- src/modules/navigator/land.cpp | 1 + src/modules/navigator/mission.cpp | 2 + src/modules/navigator/navigator_main.cpp | 1 + src/modules/navigator/rtl.cpp | 2 + 8 files changed, 54 insertions(+), 48 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 3f31673a71..b9afd344ad 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -446,6 +446,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) // activation failed if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) { + PX4_INFO("activate failed"); _current_task.task->~FlightTask(); _current_task.task = nullptr; _current_task.index = 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 3e5444a1a5..f0f5a89683 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -104,6 +104,10 @@ bool FlightTaskAuto::updateInitialize() bool FlightTaskAuto::update() { + if (_type_previous != _type) { + PX4_INFO("_type: %d", (int)_type); + } + bool ret = FlightTask::update(); // always reset constraints because they might change depending on the type _setDefaultConstraints(); @@ -324,7 +328,6 @@ 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 7204f09cf1..7528ab3127 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -52,13 +52,8 @@ static constexpr int32_t RTL_PREC_LAND_REQUIRED = 2; bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint) { - // PX4_INFO("FlightTaskAutoPrecisionLanding::activate"); - bool ret = FlightTask::activate(last_setpoint); - - _position_setpoint = _position; - _velocity_setpoint = _velocity; - _yaw_setpoint = _yaw; - _yawspeed_setpoint = 0.0f; + PX4_INFO("FlightTaskAutoPrecisionLanding::activate"); + bool ret = FlightTaskAuto::activate(last_setpoint); _search_count = 0; _last_slewrate_time = 0; @@ -74,15 +69,19 @@ bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_ // Regular precision land mode and mission precision land mode have the same behavior if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { _mode = PrecLandMode::RegularPrecisionLand; + PX4_INFO("mode: %d", (int)_mode); } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND && _param_rtl_pld_md.get() >= RTL_PREC_LAND_OPPORTUNISTIC) { _mode = PrecLandMode::RegularLandOpportunistic; + PX4_INFO("mode: %d", (int)_mode); } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_OPPORTUNISTIC) { _mode = PrecLandMode::ReturnToLaunchOpportunistic; + PX4_INFO("mode: %d", (int)_mode); } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_REQUIRED) { _mode = PrecLandMode::ReturnToLaunchRequired; + PX4_INFO("mode: %d", (int)_mode); } return ret; @@ -90,7 +89,7 @@ bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_ bool FlightTaskAutoPrecisionLanding::updateInitialize() { - bool ret = FlightTask::updateInitialize(); + bool ret = FlightTaskAuto::updateInitialize(); _sub_home_position.update(); _sub_vehicle_status.update(); @@ -112,13 +111,9 @@ bool FlightTaskAutoPrecisionLanding::updateInitialize() bool FlightTaskAutoPrecisionLanding::update() { - bool ret = FlightTask::update(); + bool ret = FlightTaskAuto::update(); - if (_landing_target_pose_sub.updated()) { - _landing_target_pose_sub.copy(&_landing_target_pose); - } - - _landing_target_valid = (hrt_elapsed_time(&_landing_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get(); + _landing_target_valid = (hrt_elapsed_time(&_landing_target_pose_sub.get().timestamp) / 1e6f) <= _param_pld_btout.get(); switch (_state) { @@ -173,9 +168,9 @@ void FlightTaskAutoPrecisionLanding::state_on_enter(PrecLandState state) _search_count++; _search_start_time = hrt_absolute_time(); break; - case PrecLandState::HorizontalApproach: - _horizontal_approach_alt = _position(2); - break; + // case PrecLandState::HorizontalApproach: + // _horizontal_approach_alt = _position(2); + // break; default: break; } @@ -189,22 +184,22 @@ void FlightTaskAutoPrecisionLanding::state_on_exit(PrecLandState state) void FlightTaskAutoPrecisionLanding::run_state_idle() { - if (_fix_this_activate_update_loop) { - _fix_this_activate_update_loop = false; - return; - } + bool is_rtl = _mode == PrecLandMode::ReturnToLaunchRequired || _mode == PrecLandMode::ReturnToLaunchOpportunistic; + bool within_home_acceptance = Vector2f(Vector2f(_sub_home_position.get().x, + _sub_home_position.get().y) - _position.xy()).norm() <= _target_acceptance_radius; - // PX4_INFO("run_state_idle: waypoint type: %d", (int)_type); + // We must wait for RTL to get home before starting + if (is_rtl && !within_home_acceptance) { + return; + } else if (is_rtl && within_home_acceptance) { + PX4_INFO("RTL reached home point"); + } switch_state(PrecLandState::Start); } void FlightTaskAutoPrecisionLanding::run_state_start() { - // Initialize our position setpoint to the current position - _position_setpoint = _position; - - if (_landing_target_valid) { switch_state(PrecLandState::HorizontalApproach); @@ -212,8 +207,11 @@ void FlightTaskAutoPrecisionLanding::run_state_start() PX4_INFO("Target not seen"); - // Check if opportunistic mode is enabled - if (_param_rtl_pld_md.get() == RTL_PREC_LAND_REQUIRED) { + bool required = _mode == PrecLandMode::RegularPrecisionLand || + _mode == PrecLandMode::MissionPrecisionLand || + _mode == PrecLandMode::ReturnToLaunchRequired; + + if (required) { switch_state(PrecLandState::Search); } else { @@ -224,13 +222,15 @@ void FlightTaskAutoPrecisionLanding::run_state_start() void FlightTaskAutoPrecisionLanding::run_state_normal_land() { - // Do nothing + if (_landing_target_valid) { + switch_state(PrecLandState::HorizontalApproach); + return; + } } 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; + _target(2) = _position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get(); // If target is seen run horizontal approach if (_landing_target_valid) { @@ -258,17 +258,16 @@ void FlightTaskAutoPrecisionLanding::run_state_horizontal_approach() return; } - float x = _landing_target_pose.x_abs; - float y = _landing_target_pose.y_abs; + float x = _landing_target_pose_sub.get().x_abs; + float y = _landing_target_pose_sub.get().y_abs; slewrate(x, y); // TODO: Replace this with PX4's AlphaFilter // Fly to target XY position, maintain current altitude - _position_setpoint(0) = x; - _position_setpoint(1) = y; - _position_setpoint(2) = _horizontal_approach_alt; + _target(0) = _position_setpoint(0) = x; + _target(1) = _position_setpoint(1) = y; // Check if it's time to descend - Vector2f target_position_vector = Vector2f(_landing_target_pose.x_abs,_landing_target_pose.y_abs); + Vector2f target_position_vector = Vector2f(_landing_target_pose_sub.get().x_abs,_landing_target_pose_sub.get().y_abs); Vector2f position_delta_vector = Vector2f(target_position_vector - _position.xy()); if (position_delta_vector.norm() <= _param_pld_hacc_rad.get() ) { @@ -283,13 +282,12 @@ void FlightTaskAutoPrecisionLanding::run_state_descend_above_target() return; } - // 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) = 0; + // FlightTask::update() function is preparing our land setpoints. + _target(0) = _position_setpoint(0) = _landing_target_pose_sub.get().x_abs; + _target(1) = _position_setpoint(1) = _landing_target_pose_sub.get().y_abs; // Check if we're within our final approach altitude - if ((_landing_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get()) { + if ((_landing_target_pose_sub.get().z_abs - _position(2)) < _param_pld_fappr_alt.get()) { switch_state(PrecLandState::Finished); } } @@ -301,8 +299,8 @@ void FlightTaskAutoPrecisionLanding::run_state_finished() 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(); + return Vector2f(Vector2f(_landing_target_pose_sub.get().x_abs, + _landing_target_pose_sub.get().y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get(); } void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &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 2e735271c8..64e8eb059f 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -70,7 +70,7 @@ enum class PrecLandMode { MissionPrecisionLand }; -class FlightTaskAutoPrecisionLanding : public FlightTask +class FlightTaskAutoPrecisionLanding : public FlightTaskAuto { public: FlightTaskAutoPrecisionLanding() = default; @@ -106,8 +106,6 @@ private: uORB::PublicationMulti _precision_landing_status_pub {ORB_ID(precision_landing_status)}; - landing_target_pose_s _landing_target_pose {}; - float _horizontal_approach_alt {}; // FIX THIS diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 28ffd36d07..aec2aee5b6 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -49,6 +49,7 @@ Land::Land(Navigator *navigator) : void Land::on_activation() { + PX4_INFO("Land::on_activation"); /* set current mission item to Land */ set_land_item(&_mission_item); _navigator->get_mission_result()->finished = false; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 5b92654114..8233048fb7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -971,6 +971,7 @@ Mission::set_mission_items() } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + PX4_INFO("new work item type is PRECISION_LAND"); new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND; } } @@ -980,6 +981,7 @@ Mission::set_mission_items() new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + PX4_INFO("we just moved to the landing waypoint, now descend"); new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7217ded69f..01c29ca6e6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -769,6 +769,7 @@ void Navigator::run() break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; break; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b0856d8ab4..66826fd8c8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -249,6 +249,8 @@ void RTL::find_RTL_destination() void RTL::on_activation() { + PX4_INFO("RTL::on_activation"); + _rtl_state = RTL_STATE_NONE; // if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode