From 2bf072e5cddbe100eacf12876ed6053db575cf2f Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Fri, 8 Apr 2022 10:47:27 +0200 Subject: [PATCH] mission: publish sub flight mode for flightmode manager --- msg/NavigatorMissionItem.msg | 5 +++ .../flight_mode_manager/FlightModeManager.cpp | 14 +++++--- .../flight_mode_manager/FlightModeManager.hpp | 2 ++ .../FlightTaskAutoPrecisionLanding.cpp | 12 ++++--- src/modules/navigator/mission.cpp | 34 ++++++++++++++++--- src/modules/navigator/mission.h | 5 ++- 6 files changed, 58 insertions(+), 14 deletions(-) diff --git a/msg/NavigatorMissionItem.msg b/msg/NavigatorMissionItem.msg index 64af762f75..65ce45cbaa 100644 --- a/msg/NavigatorMissionItem.msg +++ b/msg/NavigatorMissionItem.msg @@ -6,6 +6,11 @@ uint16 sequence_current # Sequence of the current mission item uint16 nav_cmd +uint8 MISSION_SUBMODE_NONE = 0 +uint8 MISSION_SUBMODE_PRECLAND = 1 + +uint8 nav_sub_cmd # sub-command, for example AUTO_PRECLAND in main command AUTO_MISSION + float32 latitude float32 longitude diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 0df602fa6b..3bd6247026 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -107,6 +107,7 @@ void FlightModeManager::Run() _vehicle_control_mode_sub.update(); _vehicle_land_detected_sub.update(); _vehicle_status_sub.update(); + _navigator_mission_item_sub.update(); start_flight_task(); @@ -139,6 +140,14 @@ 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; + 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::MISSION_SUBMODE_PRECLAND; + // 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); @@ -164,11 +173,8 @@ void FlightModeManager::start_flight_task() task_failure = true; } + } else if (land_should_be_precland || precland_mission_item_active) { // Take-over landing from navigator if precision landing is enabled - - } else if ((_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) { should_disable_task = false; if (switchTask(FlightTaskIndex::AutoPrecisionLanding) != FlightTaskError::NoError) { diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 5c281ddac5..ff987ca631 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +151,7 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _navigator_mission_item_sub{ORB_ID(navigator_mission_item)}; uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 7d3aec719f..169bcc0193 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -121,20 +121,24 @@ bool FlightTaskAutoPrecisionLanding::update() void FlightTaskAutoPrecisionLanding::run_state_start() { - _position_setpoint = _target; // Follow navigator triplet + // 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; - // check if target visible and go to horizontal approach directly if (switch_to_state_horizontal_approach()) { + // 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 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! switch_to_state_search(); } } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3bf632c19e..3317311d99 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -746,7 +746,7 @@ Mission::set_mission_items() user_feedback_done = true; } - publish_navigator_mission_item(); // for logging + publish_navigator_mission_item(); _navigator->set_position_setpoint_triplet_updated(); return; @@ -967,6 +967,20 @@ Mission::set_mission_items() // if the vehicle drifted off the path during back-transition it should just go straight to the landing point pos_sp_triplet->previous.valid = false; + } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) { + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + } + } + + /* we just moved to the landing waypoint, now descend */ + if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && + new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + } + } /* ignore yaw for landing items */ @@ -1100,8 +1114,10 @@ Mission::set_mission_items() } else { // The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items) // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } // ELSE: The current position setpoint stays unchanged. } @@ -1119,6 +1135,13 @@ Mission::set_mission_items() /* set current work item type */ _work_item_type = new_work_item_type; + if (new_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { + _mission_sub_mode = navigator_mission_item_s::MISSION_SUBMODE_PRECLAND; + + } else { + _mission_sub_mode = navigator_mission_item_s::MISSION_SUBMODE_NONE; + } + /* require takeoff after landing or idle */ if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { @@ -1171,7 +1194,7 @@ Mission::set_mission_items() } } - publish_navigator_mission_item(); // for logging + publish_navigator_mission_item(); _navigator->set_position_setpoint_triplet_updated(); } @@ -1418,7 +1441,7 @@ Mission::do_abort_landing() _navigator->get_position_setpoint_triplet()->next.lon = (double)NAN; _navigator->get_position_setpoint_triplet()->next.alt = NAN; - publish_navigator_mission_item(); // for logging + publish_navigator_mission_item(); _navigator->set_position_setpoint_triplet_updated(); mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", @@ -1862,6 +1885,7 @@ void Mission::publish_navigator_mission_item() navigator_mission_item.instance_count = _navigator->mission_instance_count(); navigator_mission_item.sequence_current = _current_mission_index; navigator_mission_item.nav_cmd = _mission_item.nav_cmd; + navigator_mission_item.nav_sub_cmd = _mission_sub_mode; navigator_mission_item.latitude = _mission_item.lat; navigator_mission_item.longitude = _mission_item.lon; navigator_mission_item.altitude = _mission_item.altitude; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6f15b6b96d..43ad5ca09a 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -288,9 +288,12 @@ private: WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, - WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, + WORK_ITEM_TYPE_PRECISION_LAND } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */ bool _execution_mode_changed{false}; + + uint8_t _mission_sub_mode{navigator_mission_item_s::MISSION_SUBMODE_NONE}; };