From d66f7b7b436e3870fd86e36bf052ea69a6c9919d Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Wed, 20 Apr 2022 14:19:45 +0200 Subject: [PATCH] mission: publish "work item" as current sub flight mode --- msg/NavigatorMissionItem.msg | 13 +- .../flight_mode_manager/FlightModeManager.cpp | 2 +- src/modules/navigator/mission.cpp | 111 +++++++++--------- src/modules/navigator/mission.h | 13 +- src/modules/navigator/rtl_params.c | 2 + 5 files changed, 67 insertions(+), 74 deletions(-) diff --git a/msg/NavigatorMissionItem.msg b/msg/NavigatorMissionItem.msg index 65ce45cbaa..d420166447 100644 --- a/msg/NavigatorMissionItem.msg +++ b/msg/NavigatorMissionItem.msg @@ -6,10 +6,17 @@ uint16 sequence_current # Sequence of the current mission item uint16 nav_cmd -uint8 MISSION_SUBMODE_NONE = 0 -uint8 MISSION_SUBMODE_PRECLAND = 1 +# Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message +uint8 WORK_ITEM_TYPE_DEFAULT = 0 # Default mission item +uint8 WORK_ITEM_TYPE_TAKEOFF = 1 # Takeoff before moving to waypoint +uint8 WORK_ITEM_TYPE_MOVE_TO_LAND = 2 # Move to land waypoint before descent +uint8 WORK_ITEM_TYPE_ALIGN = 3 # Align for next waypoint +uint8 WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF = 4 # VTOL specific +uint8 WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION = 5 # VTOL specific +uint8 WORK_ITEM_TYPE_PRECISION_LAND = 6 # Precision Landing for Multicopters using external aid -uint8 nav_sub_cmd # sub-command, for example AUTO_PRECLAND in main command AUTO_MISSION +uint8 nav_sub_cmd # Synonymous to "work_item_type" in Navigator. Will be renamed eventually in navigator. + # 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 3bd6247026..bfd644ec65 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -146,7 +146,7 @@ void FlightModeManager::start_flight_task() && _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; + _navigator_mission_item_sub.get().nav_sub_cmd == navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND; // 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) { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3317311d99..5b92654114 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -156,7 +156,7 @@ Mission::on_inactive() } /* reset so current mission item gets restarted if mission was paused */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; /* reset so MISSION_ITEM_REACHED isn't published */ _navigator->get_mission_result()->seq_reached = -1; @@ -179,7 +179,7 @@ Mission::on_inactivation() _time_mission_deactivated = hrt_absolute_time(); /* reset so current mission item gets restarted if mission was paused */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; } void @@ -245,7 +245,7 @@ Mission::on_active() if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { /* If we just completed a takeoff which was inserted before the right waypoint, there is no need to report that we reached it because we didn't. */ - if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) { + if (_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF) { set_mission_item_reached(); } @@ -276,7 +276,7 @@ Mission::on_active() || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND - || _work_item_type == WORK_ITEM_TYPE_ALIGN)) { + || _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN)) { // Mount control is disabled If the vehicle is in ROI-mode, the vehicle // needs to rotate such that ROI is in the field of view. // ROI only makes sense for multicopters. @@ -368,7 +368,7 @@ Mission::set_execution_mode(const uint8_t mode) --_current_mission_index; } - _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; } break; @@ -384,7 +384,7 @@ Mission::set_execution_mode(const uint8_t mode) ++_current_mission_index; } - _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; } break; @@ -536,7 +536,7 @@ Mission::update_mission() _navigator->get_mission_result()->seq_total = _mission.count; /* reset work item if new mission has been accepted */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; _mission_changed = true; } @@ -575,7 +575,7 @@ void Mission::advance_mission() { /* do not advance mission item if we're processing sub mission work items */ - if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) { + if (_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { return; } @@ -637,7 +637,7 @@ Mission::set_mission_items() bool has_next_position_item = false; bool has_after_next_position_item = false; - work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + uint8_t new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, &mission_item_after_next_position, &has_after_next_position_item)) { @@ -772,10 +772,10 @@ Mission::set_mission_items() /* do takeoff before going to setpoint if needed and not already in takeoff */ /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ if (do_need_vertical_takeoff() && - _work_item_type == WORK_ITEM_TYPE_DEFAULT && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT && + new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { - new_work_item_type = WORK_ITEM_TYPE_TAKEOFF; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF; /* use current mission item as next position item */ mission_item_next_position = _mission_item; @@ -800,8 +800,8 @@ Mission::set_mission_items() _mission_item.time_inside = 0.0f; } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ @@ -810,11 +810,11 @@ Mission::set_mission_items() _mission_item.yaw = NAN; } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { // if the vehicle is already in fixed wing mode then the current mission item // will be accepted immediately and the work items will be skipped - _work_item_type = WORK_ITEM_TYPE_TAKEOFF; + _work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ @@ -823,8 +823,8 @@ Mission::set_mission_items() /* if we just did a normal takeoff navigate to the actual waypoint now */ if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_TAKEOFF && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF && + new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ @@ -833,8 +833,8 @@ Mission::set_mission_items() /* if we just did a VTOL takeoff, prepare transition */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_TAKEOFF && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT && + _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF && + new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed) { @@ -848,7 +848,7 @@ Mission::set_mission_items() _mission_item.force_heading = true; - new_work_item_type = WORK_ITEM_TYPE_ALIGN; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN; /* set position setpoint to current while aligning */ _mission_item.lat = _navigator->get_global_position()->lat; @@ -857,7 +857,7 @@ Mission::set_mission_items() /* heading is aligned now, prepare transition */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_ALIGN && + _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed) { @@ -866,7 +866,7 @@ Mission::set_mission_items() /* check if the vtol_takeoff waypoint is on top of us */ if (do_need_move_to_takeoff()) { - new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF; } set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); @@ -878,10 +878,10 @@ Mission::set_mission_items() /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF + && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; @@ -889,11 +889,12 @@ Mission::set_mission_items() /* move to land wp as fixed wing */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT + || _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) + && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT && !_navigator->get_land_detected()->landed) { - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND; /* use current mission item as next position item */ mission_item_next_position = _mission_item; @@ -915,8 +916,8 @@ Mission::set_mission_items() /* transition to MC */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND + && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_navigator->get_land_detected()->landed) { @@ -924,7 +925,7 @@ Mission::set_mission_items() _mission_item.altitude = _navigator->get_global_position()->alt; _mission_item.altitude_is_relative = false; - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; // make previous setpoint invalid, such that there will be no prev-current line following // if the vehicle drifted off the path during back-transition it should just go straight to the landing point @@ -933,11 +934,11 @@ Mission::set_mission_items() /* move to landing waypoint before descent if necessary */ if (do_need_move_to_land() && - (_work_item_type == WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) && + new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND; /* use current mission item as next position item */ mission_item_next_position = _mission_item; @@ -967,18 +968,19 @@ 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) { + } 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) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + new_work_item_type = navigator_mission_item_s::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 (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND && + new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND; } } @@ -1031,8 +1033,8 @@ Mission::set_mission_items() /* turn towards next waypoint before MC to FW transition */ if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT + && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed && has_next_position_item) { @@ -1040,7 +1042,7 @@ Mission::set_mission_items() /* disable weathervane before front transition for allowing yaw to align */ pos_sp_triplet->current.disable_weather_vane = true; - new_work_item_type = WORK_ITEM_TYPE_ALIGN; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN; set_align_mission_item(&_mission_item, &mission_item_next_position); @@ -1050,10 +1052,10 @@ Mission::set_mission_items() } /* yaw is aligned now */ - if (_work_item_type == WORK_ITEM_TYPE_ALIGN && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + if (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN && + new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) { - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT; /* re-enable weather vane again after alignment */ pos_sp_triplet->current.disable_weather_vane = false; @@ -1114,7 +1116,7 @@ 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 - if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { + if (new_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND) { mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } @@ -1124,7 +1126,7 @@ Mission::set_mission_items() // Only set the previous position item if the current one really changed // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) && + if ((_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND) && !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { pos_sp_triplet->previous = current_setpoint_copy; } @@ -1135,13 +1137,6 @@ 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) { @@ -1885,7 +1880,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.nav_sub_cmd = _work_item_type; 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 43ad5ca09a..ac0c362106 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -281,19 +281,8 @@ private: bool _mission_waypoints_changed{false}; bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */ - // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message - enum work_item_type { - WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ - WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ - 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_PRECISION_LAND - } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ + uint8_t _work_item_type{navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do during mission (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}; }; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index ef78a37020..766959e93e 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -135,6 +135,8 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0); */ PARAM_DEFINE_INT32(RTL_CONE_ANG, 45); + +// TODO: Rename to NAV_PLD_MD if it also applies to landing rather than only RTL /** * RTL precision land mode *