From c1214c847fe29dcf801a68e34ebc4bfa890f6e9d Mon Sep 17 00:00:00 2001 From: Konrad Date: Thu, 26 Oct 2023 09:42:00 +0200 Subject: [PATCH] rtl+mission: remove do_need_move_to_land and handleLanding duplicated code to reduce flash --- src/modules/navigator/mission.cpp | 147 +----------------- src/modules/navigator/mission.h | 7 - src/modules/navigator/mission_base.cpp | 123 +++++++++++++++ src/modules/navigator/mission_base.h | 18 +++ src/modules/navigator/mission_block.cpp | 12 ++ src/modules/navigator/mission_block.h | 2 + src/modules/navigator/rtl_direct.cpp | 9 +- .../navigator/rtl_direct_mission_land.cpp | 120 ++++---------- .../navigator/rtl_direct_mission_land.h | 2 - src/modules/navigator/rtl_mission_fast.cpp | 134 ++++------------ src/modules/navigator/rtl_mission_fast.h | 4 - .../navigator/rtl_mission_fast_reverse.cpp | 19 +-- .../navigator/rtl_mission_fast_reverse.h | 2 - 13 files changed, 215 insertions(+), 384 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9daa37a733..0853d8cce7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -155,21 +155,6 @@ bool Mission::setNextMissionItem() return (goToNextItem(true) == PX4_OK); } -bool -Mission::do_need_move_to_land() -{ - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { - - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _global_pos_sub.get().lat, _global_pos_sub.get().lon); - - return d_current > _navigator->get_acceptance_radius(); - } - - return false; -} - bool Mission::do_need_move_to_takeoff() { @@ -188,7 +173,7 @@ Mission::do_need_move_to_takeoff() void Mission::setActiveMissionItems() { /* Get mission item that comes after current if available */ - constexpr static size_t max_num_next_items{2u}; + static constexpr size_t max_num_next_items{2u}; int32_t next_mission_items_index[max_num_next_items]; size_t num_found_items; @@ -218,10 +203,6 @@ void Mission::setActiveMissionItems() const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; if (item_contains_position(_mission_item)) { - /* force vtol land */ - if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { - _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; - } handleTakeoff(new_work_item_type, next_mission_items, num_found_items); @@ -425,132 +406,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex } } -void Mission::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], - size_t &num_found_items) -{ - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* move to land wp as fixed wing */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT - || _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) - && !_land_detected_sub.get().landed) { - - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - num_found_items = 1u; - next_mission_items[0u] = _mission_item; - - float altitude = _global_pos_sub.get().alt; - - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - _mission_item.vtol_back_transition = true; - } - - /* transition to MC */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_land_detected_sub.get().landed) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _mission_item.altitude = _global_pos_sub.get().alt; - _mission_item.altitude_is_relative = false; - - new_work_item_type = WorkItemType::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 - pos_sp_triplet->previous.valid = false; - } - - /* move to landing waypoint before descent if necessary */ - if (do_need_move_to_land() && - (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { - - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - num_found_items = 1u; - next_mission_items[0u] = _mission_item; - - /* - * Ignoring waypoint altitude: - * Set altitude to the same as we have now to prevent descending too fast into - * the ground. Actual landing will descend anyway until it touches down. - * XXX: We might want to change that at some point if it is clear to the user - * what the altitude means on this waypoint type. - */ - float altitude = _global_pos_sub.get().alt; - - if (pos_sp_triplet->current.valid - && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - - // 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 - pos_sp_triplet->previous.valid = false; - - } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - } - } - - /* we just moved to the landing waypoint, now descend */ - if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { - - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - } - } - - /* ignore yaw for landing items */ - /* XXX: if specified heading for landing is desired we could add another step before the descent - * that aligns the vehicle first */ - if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { - _mission_item.yaw = NAN; - } -} - void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e8c33e1f0e..7299e065e5 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -73,11 +73,6 @@ private: bool setNextMissionItem() override; - /** - * Returns true if we need to move to waypoint location before starting descent - */ - bool do_need_move_to_land(); - /** * Returns true if we need to move to waypoint location after vtol takeoff */ @@ -97,8 +92,6 @@ private: void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 41ce19c2cc..bb03dd996a 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -446,6 +446,11 @@ MissionBase::set_mission_items() /* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */ loadCurrentMissionItem(); + /* force vtol land */ + if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; + } + setActiveMissionItems(); } else { @@ -517,6 +522,124 @@ MissionBase::set_mission_result() _navigator->set_mission_result_updated(); } +bool MissionBase::do_need_move_to_item() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); +} + +void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) && + !_land_detected_sub.get().landed; + + /* move to land wp as fixed wing */ + if (needs_vtol_landing) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + + new_work_item_type = WorkItemType::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 + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_item() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + /* + * Ignoring waypoint altitude: + * Set altitude to the same as we have now to prevent descending too fast into + * the ground. Actual landing will descend anyway until it touches down. + * XXX: We might want to change that at some point if it is clear to the user + * what the altitude means on this waypoint type. + */ + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid + && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + + // have to reset here because these field were used in set_vtol_transition_item + _mission_item.time_inside = 0.f; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + + // 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 + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } else { + + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + + startPrecLand(_mission_item.land_precision); + } + } + } + + /* ignore yaw for landing items */ + /* XXX: if specified heading for landing is desired we could add another step before the descent + * that aligns the vehicle first */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + _mission_item.yaw = NAN; + } +} + bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const { return ((p1->valid == p2->valid) && diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 287b7d3b88..660c918686 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -279,6 +279,24 @@ protected: * */ void publish_navigator_mission_item(); + + /** + * @brief Do need move to item + * + * @return true if the item is horizontally further away than the mission item + * @return false otherwise + */ + bool do_need_move_to_item(); + + /** + * @brief Handle landing + * + * @param new_work_item_type new work item type state machine to be set + * @param next_mission_items the next mission items after the current mission item + * @param num_found_items number of found next mission items + */ + void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items); /** * @brief I position setpoint equal * diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8c0654f8a8..b56511fa33 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -1059,3 +1059,15 @@ float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, Headi return desired_yaw; } + +void MissionBlock::startPrecLand(uint16_t land_precision) +{ + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _navigator->get_precland()->on_activation(); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + _navigator->get_precland()->on_activation(); + } +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 9b79ea6790..b95ff56c7f 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -223,6 +223,8 @@ protected: float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const; + void startPrecLand(uint16_t land_precision); + /** * @brief Issue a command for mission items with a nav_cmd that specifies an action * diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index c29df4327a..fd578c5c8c 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -322,14 +322,7 @@ void RtlDirect::set_rtl_item() _mission_item.land_precision = _param_rtl_pld_md.get(); - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - _navigator->get_precland()->on_activation(); - - } else if (_mission_item.land_precision == 2) { - _navigator->get_precland()->set_mode(PrecLandMode::Required); - _navigator->get_precland()->on_activation(); - } + startPrecLand(_mission_item.land_precision); _rtl_state = RTLState::IDLE; diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 236b7b61c5..597af4ad9d 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -140,9 +140,32 @@ void RtlDirectMissionLand::setActiveMissionItems() new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; } else if (item_contains_position(_mission_item)) { + + static constexpr size_t max_num_next_items{1u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); + + mission_item_s next_mission_items[max_num_next_items]; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (size_t i = 0U; i < num_found_items; i++) { + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i], + reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + next_mission_items[i] = next_mission_item; + + } else { + num_found_items = i; + break; + } + } + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { - handleLanding(new_work_item_type); + handleLanding(new_work_item_type, next_mission_items, num_found_items); } else { // convert mission item to a simple waypoint, keep loiter to alt @@ -156,21 +179,9 @@ void RtlDirectMissionLand::setActiveMissionItems() pos_sp_triplet->previous = pos_sp_triplet->current; } - int32_t next_mission_item_index; - size_t num_found_items = 0; - getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); - if (num_found_items > 0) { - - const dm_item_t dataman_id = static_cast(_mission.dataman_id); - mission_item_s next_mission_item; - bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, - reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); - - if (success) { - mission_apply_limitation(next_mission_item); - mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); - } + mission_apply_limitation(next_mission_items[0u]); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); } mission_apply_limitation(_mission_item); @@ -192,85 +203,6 @@ void RtlDirectMissionLand::setActiveMissionItems() _navigator->set_position_setpoint_triplet_updated(); } -void RtlDirectMissionLand::handleLanding(WorkItemType &new_work_item_type) -{ - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - bool needs_to_land = !_land_detected_sub.get().landed && - ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) - || (_mission_item.nav_cmd == NAV_CMD_LAND)); - bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && - (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && - needs_to_land; - - if (needs_vtol_landing) { - if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; - - float altitude = _global_pos_sub.get().alt; - - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - _mission_item.vtol_back_transition = true; - - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - - } - - /* transition to MC */ - if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _mission_item.altitude = _global_pos_sub.get().alt; - _mission_item.altitude_is_relative = false; - _mission_item.yaw = NAN; - - new_work_item_type = WorkItemType::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 - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - } - - } else if (needs_to_land) { - /* move to landing waypoint before descent if necessary */ - if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && - do_need_move_to_land() && - (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { - - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; - - _mission_item.altitude = _global_pos_sub.get().alt; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - // 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 - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - - } - } -} - -bool RtlDirectMissionLand::do_need_move_to_land() -{ - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _global_pos_sub.get().lat, _global_pos_sub.get().lon); - - return d_current > _navigator->get_acceptance_radius(); - -} - rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() { rtl_time_estimate_s time_estimate; diff --git a/src/modules/navigator/rtl_direct_mission_land.h b/src/modules/navigator/rtl_direct_mission_land.h index 002d26da67..5edeafc71e 100644 --- a/src/modules/navigator/rtl_direct_mission_land.h +++ b/src/modules/navigator/rtl_direct_mission_land.h @@ -65,8 +65,6 @@ public: private: bool setNextMissionItem() override; void setActiveMissionItems() override; - void handleLanding(WorkItemType &new_work_item_type); - bool do_need_move_to_land(); bool _needs_climbing{false}; //< Flag if climbing is required at the start bool _enforce_rtl_alt{false}; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index 924ef8b8ba..acb16d2d62 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -54,6 +54,8 @@ RtlMissionFast::RtlMissionFast(Navigator *navigator) : void RtlMissionFast::on_activation() { + _home_pos_sub.update(); + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; @@ -65,18 +67,6 @@ void RtlMissionFast::on_activation() MissionBase::on_activation(); } -void RtlMissionFast::on_active() -{ - _home_pos_sub.update(); - MissionBase::on_active(); -} - -void RtlMissionFast::on_inactive() -{ - _home_pos_sub.update(); - MissionBase::on_inactive(); -} - bool RtlMissionFast::setNextMissionItem() { return (goToNextPositionItem(true) == PX4_OK); @@ -100,9 +90,32 @@ void RtlMissionFast::setActiveMissionItems() new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; } else if (item_contains_position(_mission_item)) { + + static constexpr size_t max_num_next_items{1u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); + + mission_item_s next_mission_items[max_num_next_items]; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (size_t i = 0U; i < num_found_items; i++) { + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i], + reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + next_mission_items[i] = next_mission_item; + + } else { + num_found_items = i; + break; + } + } + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { - handleLanding(new_work_item_type); + handleLanding(new_work_item_type, next_mission_items, num_found_items); } else { // convert mission item to a simple waypoint, keep loiter to alt @@ -116,21 +129,11 @@ void RtlMissionFast::setActiveMissionItems() pos_sp_triplet->previous = pos_sp_triplet->current; } - int32_t next_mission_item_index; - size_t num_found_items = 0; - getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); + if (num_found_items > 0) { - - const dm_item_t dataman_id = static_cast(_mission.dataman_id); - mission_item_s next_mission_item; - bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, - reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); - - if (success) { - mission_apply_limitation(next_mission_item); - mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); - } + mission_apply_limitation(next_mission_items[0u]); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); } mission_apply_limitation(_mission_item); @@ -152,85 +155,6 @@ void RtlMissionFast::setActiveMissionItems() _navigator->set_position_setpoint_triplet_updated(); } -void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type) -{ - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - bool needs_to_land = !_land_detected_sub.get().landed && - ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) - || (_mission_item.nav_cmd == NAV_CMD_LAND)); - bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && - (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && - needs_to_land; - - if (needs_vtol_landing) { - if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; - - float altitude = _global_pos_sub.get().alt; - - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - _mission_item.vtol_back_transition = true; - - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - - } - - /* transition to MC */ - if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _mission_item.altitude = _global_pos_sub.get().alt; - _mission_item.altitude_is_relative = false; - _mission_item.yaw = NAN; - - new_work_item_type = WorkItemType::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 - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - } - - } else if (needs_to_land) { - /* move to landing waypoint before descent if necessary */ - if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && - do_need_move_to_land() && - (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { - - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; - - _mission_item.altitude = _global_pos_sub.get().alt; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - // 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 - _navigator->reset_position_setpoint(pos_sp_triplet->previous); - - } - } -} - -bool RtlMissionFast::do_need_move_to_land() -{ - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _global_pos_sub.get().lat, _global_pos_sub.get().lon); - - return d_current > _navigator->get_acceptance_radius(); - -} - rtl_time_estimate_s RtlMissionFast::calc_rtl_time_estimate() { rtl_time_estimate_s time_estimate; diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h index 576152efe2..bb3db38c64 100644 --- a/src/modules/navigator/rtl_mission_fast.h +++ b/src/modules/navigator/rtl_mission_fast.h @@ -56,16 +56,12 @@ public: ~RtlMissionFast() = default; void on_activation() override; - void on_active() override; - void on_inactive() override; rtl_time_estimate_s calc_rtl_time_estimate() override; private: bool setNextMissionItem() override; void setActiveMissionItems() override; - void handleLanding(WorkItemType &new_work_item_type); - bool do_need_move_to_land(); uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ }; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 1b5feb850d..08188a9edb 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -54,6 +54,8 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : void RtlMissionFastReverse::on_activation() { + _home_pos_sub.update(); + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; @@ -71,12 +73,6 @@ void RtlMissionFastReverse::on_active() MissionBase::on_active(); } -void RtlMissionFastReverse::on_inactive() -{ - _home_pos_sub.update(); - MissionBase::on_inactive(); -} - bool RtlMissionFastReverse::setNextMissionItem() { return (goToPreviousPositionItem(true) == PX4_OK); @@ -227,7 +223,7 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) _mission_item.yaw = NAN; if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && - do_need_move_to_land()) { + do_need_move_to_item()) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; _mission_item.altitude = _global_pos_sub.get().alt; @@ -249,15 +245,6 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) } } -bool RtlMissionFastReverse::do_need_move_to_land() -{ - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _global_pos_sub.get().lat, _global_pos_sub.get().lon); - - return d_current > _navigator->get_acceptance_radius(); - -} - rtl_time_estimate_s RtlMissionFastReverse::calc_rtl_time_estimate() { rtl_time_estimate_s time_estimate; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index acc2893b3a..889dd8716b 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -57,7 +57,6 @@ public: void on_activation() override; void on_active() override; - void on_inactive() override; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -65,7 +64,6 @@ private: bool setNextMissionItem() override; void setActiveMissionItems() override; void handleLanding(WorkItemType &new_work_item_type); - bool do_need_move_to_land(); uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ };