diff --git a/docs/en/flight_modes/return.md b/docs/en/flight_modes/return.md index 2b3bf4389d..64d3a27d56 100644 --- a/docs/en/flight_modes/return.md +++ b/docs/en/flight_modes/return.md @@ -110,11 +110,13 @@ The behaviour is fairly complex because it depends on the flight mode, and wheth Mission _with_ landing pattern: -- **Mission mode:** Mission is continued in "fast-forward mode" (jumps, delay and any other non-position commands ignored, loiter and other position waypoints converted to simple waypoints) and then lands. +- **Mission mode:** + - Mission is continued in "fast-forward mode" and then lands. + - DO_JUMP commands, delays and other non-position mission items are ignored, and loiter and other position waypoints are converted to simple waypoints. - **Auto mode other than mission mode:** - Ascend to a safe [minimum return altitude](#minimum-return-altitude) above any expected obstacles. - Fly directly to closest waypoint (for FW not a landing WP) and descend to waypoint altitude. - - Continue mission in fast forward mode from that waypoint. + - Continue mission in fast forward mode from that waypoint, using the same traversal rules as above. - **Manual modes:** - Ascend to a safe [minimum return altitude](#minimum-return-altitude) above any expected obstacles. - Fly directly to landing sequence position and descend to waypoint altitude @@ -124,7 +126,7 @@ Mission _without_ landing pattern defined: - **Mission mode:** - Mission flown "fast-backward" (in reverse) starting from the previous waypoint - - Jumps, delay and any other non-position commands ignored, loiter and other position waypoints converted to simple waypoints. + - DO_JUMP commands, delays and other non-position mission items are ignored, and loiter and other position waypoints are converted to simple waypoints. - VTOL vehicles transition to FW mode (if needed) before flying the mission in reverse. - On reaching waypoint 1, the vehicle ascends to the [minimum return altitude](#minimum-return-altitude) and flies to the home position (where it [lands or waits](#loiter-landing-at-destination)). - **Auto mode other than mission mode:** @@ -136,6 +138,11 @@ If no mission is defined PX4 will fly directly to home location and land (rally If the mission changes during return mode, then the behaviour is re-evaluated based on the new mission following the same rules as above (e.g. if the new mission has no landing sequence and you're in a mission, the mission is reversed). +::: info +For `RTL_TYPE=4`, PX4 currently chooses between continuing to a mission landing and reversing toward home by comparing raw mission item indices. +This is only an approximation of the flown path length, because the number if mission items is not indicative of the distance remaining and non-position items are also counted. +::: + ### Closest Safe Destination Return Type (RTL_TYPE=3) In this return type the vehicle: @@ -207,7 +214,7 @@ The RTL parameters are listed in [Parameter Reference > Return Mode](../advanced | Parameter | Description | | ----------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| [RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) | Return mechanism (path and destination).
`0`: Return to a rally point or home (whichever is closest) via direct path.
`1`: Return to a rally point or the mission landing pattern start point (whichever is closest), via direct path. If neither mission landing or rally points are defined return home via a direct path. If the destination is a mission landing pattern, follow the pattern to land.
`2`: Use mission path fast-forward to landing if a landing pattern is defined, otherwise fast-reverse to home. Ignore rally points. Fly direct to home if no mission plan is defined.
`3`: Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. | +| [RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) | Return mechanism (path and destination).
`0`: Return to a rally point or home (whichever is closest) via direct path.
`1`: Return to a rally point or the mission landing pattern start point (whichever is closest), via direct path. If neither mission landing or rally points are defined return home via a direct path. If the destination is a mission landing pattern, follow the pattern to land.
`2`: Use the mission path to landing while skipping DO_JUMP and other non-position mission items if a landing pattern is defined, otherwise fast-reverse to home with the same traversal rules. Ignore rally points. Fly direct to home if no mission plan is defined.
`3`: Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. | | [RTL_RETURN_ALT](../advanced_config/parameter_reference.md#RTL_RETURN_ALT) | Return altitude in meters (default: 60m) when [RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) is 0. If already above this value the vehicle will return at its current altitude. | | [RTL_DESCEND_ALT](../advanced_config/parameter_reference.md#RTL_DESCEND_ALT) | Minimum return altitude and altitude at which the vehicle will slow or stop its initial descent from a higher return altitude (default: 30m) | | [RTL_LAND_DELAY](../advanced_config/parameter_reference.md#RTL_LAND_DELAY) | Time to wait at `RTL_DESCEND_ALT` before landing (default: 0.5s) -by default this period is short so that the vehicle will simply slow and then land immediately. If set to -1 the system will loiter at `RTL_DESCEND_ALT` rather than landing. The delay is provided to allow you to configure time for landing gear to be deployed (triggered automatically). | diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 743fdfa96c..a0ba7e57c6 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -34,6 +34,10 @@ add_subdirectory(GeofenceBreachAvoidance) add_subdirectory(MissionFeasibility) +if(BUILD_TESTING) + add_subdirectory(test) +endif() + set(NAVIGATOR_SOURCES navigator_main.cpp navigator_mode.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c6fcb143a1..60f5a36c92 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -99,7 +99,7 @@ Mission::set_current_mission_index(uint16_t index) } if (_navigator->get_mission_result()->valid && (index < _mission.count)) { - if (goToItem(index, true) != PX4_OK) { + if (goToItem(index, MissionTraversalType::FollowMissionControlFlow) != PX4_OK) { // Keep the old mission index (it was not updated by the interface) and report back. return false; } @@ -130,7 +130,7 @@ Mission::set_current_mission_index(uint16_t index) bool Mission::setNextMissionItem() { - return (goToNextItem(true) == PX4_OK); + return (goToNextItem() == PX4_OK); } bool diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 55cc10036b..42da3be52c 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -290,9 +290,7 @@ MissionBase::on_active() if (num_found_items == 1U && !PX4_ISFINITE(_mission_item.yaw)) { mission_item_s next_position_mission_item; - const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); - bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_item_index, - reinterpret_cast(&next_position_mission_item), sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT); + const bool success = loadMissionItemFromCache(next_mission_item_index, next_position_mission_item); if (success) { _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, @@ -535,9 +533,7 @@ MissionBase::set_mission_items() bool MissionBase::loadCurrentMissionItem() { - const dm_item_t dm_item = static_cast(_mission.mission_dataman_id); - bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast(&_mission_item), - sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + const bool success = loadMissionItemFromCache(_mission.current_seq, _mission_item); if (!success) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item could not be set.\t"); @@ -913,7 +909,7 @@ MissionBase::do_abort_landing() } else { // move mission index back (landing approach point) - _is_current_planned_mission_item_valid = (goToPreviousItem(false) == PX4_OK); + _is_current_planned_mission_item_valid = (goToPreviousItem(MissionTraversalType::IgnoreDoJump) == PX4_OK); } // send reposition cmd to get out of mission @@ -971,21 +967,19 @@ bool MissionBase::isMissionValid() const return ret_val; } -int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, +int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, MissionTraversalType traversal_type, bool write_jumps, bool mission_direction_backward) { if (mission_index >= _mission.count || mission_index < 0) { return PX4_ERROR; } - const dm_item_t mission_dataman_id = (dm_item_t)_mission.mission_dataman_id; int32_t new_mission_index{mission_index}; mission_item_s new_mission; for (uint16_t jump_count = 0u; jump_count < MAX_JUMP_ITERATION; jump_count++) { /* read mission item from datamanager */ - bool success = _dataman_cache.loadWait(mission_dataman_id, new_mission_index, reinterpret_cast(&new_mission), - sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + bool success = loadMissionItemFromCache(new_mission_index, new_mission); if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -1001,8 +995,10 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, return PX4_ERROR; } - if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) { + if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) + && traversal_type == MissionTraversalType::FollowMissionControlFlow) { if (write_jumps) { + const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); new_mission.do_jump_current_count++; success = _dataman_cache.writeWait(mission_dataman_id, new_mission_index, reinterpret_cast(&new_mission), sizeof(struct mission_item_s)); @@ -1040,11 +1036,11 @@ int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, return PX4_OK; } -int MissionBase::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward) +int MissionBase::goToItem(int32_t index, MissionTraversalType traversal_type, bool mission_direction_backward) { mission_item_s mission_item; - if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == PX4_OK) { + if (getNonJumpItem(index, mission_item, traversal_type, true, mission_direction_backward) == PX4_OK) { setMissionIndex(index); return PX4_OK; @@ -1062,29 +1058,94 @@ void MissionBase::setMissionIndex(int32_t index) } } +bool MissionBase::loadMissionItemFromCache(int32_t index, mission_item_s &mission_item) +{ + return index >= 0 + && index < _mission.count + && _dataman_cache.loadWait(static_cast(_mission.mission_dataman_id), index, + reinterpret_cast(&mission_item), sizeof(mission_item), + MAX_DATAMAN_LOAD_WAIT); +} + +bool MissionBase::findNextPositionIndex(int32_t start_index, int32_t &next_index, + MissionTraversalType traversal_type) +{ + for (int32_t mission_index = start_index; mission_index < _mission.count;) { + int32_t traversed_index = mission_index; + mission_item_s mission_item{}; + + if (!loadTraversalItem(traversed_index, mission_item, traversal_type, false)) { + return false; + } + + if (item_contains_position(mission_item)) { + next_index = traversed_index; + return true; + } + + mission_index = traversed_index + 1; + } + + return false; +} + +bool MissionBase::findPreviousPositionIndex(int32_t start_index, int32_t &previous_index, + MissionTraversalType traversal_type) +{ + for (int32_t mission_index = start_index - 1; mission_index >= 0;) { + int32_t traversed_index = mission_index; + mission_item_s mission_item{}; + + if (!loadTraversalItem(traversed_index, mission_item, traversal_type, true)) { + return false; + } + + if (item_contains_position(mission_item)) { + previous_index = traversed_index; + return true; + } + + mission_index = traversed_index - 1; + } + + return false; +} + +bool MissionBase::loadTraversalItem(int32_t &mission_index, mission_item_s &mission_item, + MissionTraversalType traversal_type, bool direction_backward) +{ + if (traversal_type == MissionTraversalType::FollowMissionControlFlow) { + return getNonJumpItem(mission_index, mission_item, MissionTraversalType::FollowMissionControlFlow, + false, direction_backward) == PX4_OK; + } + + return loadMissionItemFromCache(mission_index, mission_item); +} + void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, uint8_t max_num_items) +{ + getPreviousPositionItems(start_index, items_index, num_found_items, max_num_items, traversalType()); +} + +void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items, MissionTraversalType traversal_type) { num_found_items = 0u; - int32_t next_mission_index{start_index}; + int32_t search_index{start_index}; for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { - if (next_mission_index < 0) { + if (search_index < 0) { break; } - mission_item_s next_mission_item; - bool found_next_item{false}; + int32_t previous_position_index{-1}; - do { - next_mission_index--; - found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == PX4_OK; - } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); - - if (found_next_item) { - items_index[item_idx] = next_mission_index; + if (findPreviousPositionIndex(search_index, previous_position_index, traversal_type)) { + items_index[item_idx] = previous_position_index; num_found_items = item_idx + 1; + search_index = previous_position_index; } else { break; @@ -1094,29 +1155,30 @@ void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_in void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, uint8_t max_num_items) +{ + getNextPositionItems(start_index, items_index, num_found_items, max_num_items, traversalType()); +} + +void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items, + MissionTraversalType traversal_type) { // Make sure vector does not contain any preexisting elements. num_found_items = 0u; - int32_t next_mission_index{start_index}; + int32_t search_index{start_index}; for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { - if (next_mission_index >= _mission.count) { + if (search_index >= _mission.count) { break; } - mission_item_s next_mission_item; - bool found_next_item{false}; + int32_t next_position_index{-1}; - do { - found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == PX4_OK; - next_mission_index++; - } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); - - if (found_next_item) { - items_index[item_idx] = math::max(next_mission_index - 1, - static_cast(0)); // subtract 1 to get the index of the first position item + if (findNextPositionIndex(search_index, next_position_index, traversal_type)) { + items_index[item_idx] = next_position_index; num_found_items = item_idx + 1; + search_index = next_position_index + 1; } else { break; @@ -1124,31 +1186,44 @@ void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[ } } -int MissionBase::goToNextItem(bool execute_jump) +int MissionBase::goToNextItem() +{ + return goToNextItem(traversalType()); +} + +int MissionBase::goToNextItem(MissionTraversalType traversal_type) { if (_mission.current_seq + 1 >= (_mission.count)) { return PX4_ERROR; } - return goToItem(_mission.current_seq + 1, execute_jump); + return goToItem(_mission.current_seq + 1, traversal_type); } -int MissionBase::goToPreviousItem(bool execute_jump) +int MissionBase::goToPreviousItem() +{ + return goToPreviousItem(traversalType()); +} + +int MissionBase::goToPreviousItem(MissionTraversalType traversal_type) { if (_mission.current_seq <= 0) { return PX4_ERROR; } - return goToItem(_mission.current_seq - 1, execute_jump, true); + return goToItem(_mission.current_seq - 1, traversal_type, true); } -int MissionBase::goToPreviousPositionItem(bool execute_jump) +int MissionBase::goToPreviousPositionItem() { - size_t num_found_items{0U}; - int32_t previous_position_item_index; - getPreviousPositionItems(_mission.current_seq, &previous_position_item_index, num_found_items, 1); + return goToPreviousPositionItem(traversalType()); +} - if (num_found_items == 1U) { +int MissionBase::goToPreviousPositionItem(MissionTraversalType traversal_type) +{ + int32_t previous_position_item_index{-1}; + + if (findPreviousPositionIndex(_mission.current_seq, previous_position_item_index, traversal_type)) { setMissionIndex(previous_position_item_index); return PX4_OK; @@ -1157,13 +1232,16 @@ int MissionBase::goToPreviousPositionItem(bool execute_jump) } } -int MissionBase::goToNextPositionItem(bool execute_jump) +int MissionBase::goToNextPositionItem() { - size_t num_found_items{0U}; - int32_t next_position_item_index; - getNextPositionItems(_mission.current_seq + 1, &next_position_item_index, num_found_items, 1); + return goToNextPositionItem(traversalType()); +} - if (num_found_items == 1U) { +int MissionBase::goToNextPositionItem(MissionTraversalType traversal_type) +{ + int32_t next_position_item_index{-1}; + + if (findNextPositionIndex(_mission.current_seq + 1, next_position_item_index, traversal_type)) { setMissionIndex(next_position_item_index); return PX4_OK; @@ -1177,13 +1255,11 @@ int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, floa { int32_t min_dist_index(-1); float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); - const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) { mission_item_s mission; - bool success = _dataman_cache.loadWait(mission_dataman_id, mission_item_index, reinterpret_cast(&mission), - sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + const bool success = loadMissionItemFromCache(mission_item_index, mission); if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -1254,8 +1330,7 @@ void MissionBase::resetMissionJumpCounter() for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) { mission_item_s mission_item; - bool success = _dataman_client.readSync(mission_dataman_id, mission_index, reinterpret_cast(&mission_item), - sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + const bool success = loadMissionItemFromCache(mission_index, mission_item); if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -1385,11 +1460,8 @@ void MissionBase::updateCachedItemsUpToIndex(const int end_index) { for (int i = 0; i <= end_index; i++) { mission_item_s mission_item; - const dm_item_t dm_current = (dm_item_t)_mission.mission_dataman_id; - bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&mission_item), - sizeof(mission_item), 500_ms); - if (success) { + if (loadMissionItemFromCache(i, mission_item)) { cacheItem(mission_item); } } @@ -1415,13 +1487,10 @@ void MissionBase::checkClimbRequired(int32_t mission_item_index) if (num_found_items > 0U) { - const dm_item_t mission_dataman_id = static_cast(_mission.mission_dataman_id); mission_item_s mission; _mission_init_climb_altitude_amsl = NAN; // default to NAN, overwrite below if applicable - const bool success = _dataman_cache.loadWait(mission_dataman_id, next_mission_item_index, - reinterpret_cast(&mission), - sizeof(mission), MAX_DATAMAN_LOAD_WAIT); + const bool success = loadMissionItemFromCache(next_mission_item_index, mission); const bool is_fw_and_takeoff = mission.nav_cmd == NAV_CMD_TAKEOFF && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 982474ba69..3b648a9e49 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -99,26 +99,58 @@ protected: MISSION_TYPE_MISSION } _mission_type{MissionType::MISSION_TYPE_NONE}; + enum class MissionTraversalType : uint8_t { + FollowMissionControlFlow = 0, + IgnoreDoJump, + }; + /** - * @brief Get the Previous Mission Position Items + * @brief Get the previous mission position items using this mode's traversal policy. * - * @param[in] start_index is the index from where to start searching the previous mission position items - * @param[out] items_index is an array of indexes indicating the previous mission position items found - * @param[out] num_found_items are the amount of previous position items found - * @param[in] max_num_items are the maximum amount of previous position items to be searched + * Convenience overload for callers that want the navigation mode default. Keep the + * explicit traversal overload below for cases that intentionally need different semantics. + * + * @param[in] start_index Index from which to start searching backward + * @param[out] items_index Array of the previous position item indices + * @param[out] num_found_items Number of position items found + * @param[in] max_num_items Maximum number of position items to collect */ void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, uint8_t max_num_items); /** - * @brief Get the next mission item containing a position setpoint + * @brief Get the previous mission position items using an explicit traversal policy. * - * @param[in] start_index is the index from where to start searching (first possible return index) - * @param[out] items_index is an array of indexes indicating the next mission position items found - * @param[out] num_found_items are the amount of next position items found - * @param[in] max_num_items are the maximum amount of next position items to be searched + * @param[in] start_index Index from which to start searching backward + * @param[out] items_index Array of the previous position item indices + * @param[out] num_found_items Number of position items found + * @param[in] max_num_items Maximum number of position items to collect + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + */ + void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, + uint8_t max_num_items, + MissionTraversalType traversal_type); + /** + * @brief Get the next mission item containing a position setpoint using this mode's traversal policy. + * + * @param[in] start_index Index from which to start searching forward + * @param[out] items_index Array of the next position item indices + * @param[out] num_found_items Number of position items found + * @param[in] max_num_items Maximum number of position items to collect */ void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, uint8_t max_num_items); + /** + * @brief Get the next mission item containing a position setpoint using an explicit traversal policy. + * + * @param[in] start_index Index from which to start searching forward + * @param[out] items_index Array of the next position item indices + * @param[out] num_found_items Number of position items found + * @param[in] max_num_items Maximum number of position items to collect + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + */ + void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, + uint8_t max_num_items, + MissionTraversalType traversal_type); /** * @brief Mission has a land start, a land, and is valid * @@ -127,43 +159,71 @@ protected: */ bool hasMissionLandStart() const { return _mission.land_start_index >= 0 && _mission.land_index >= 0 && isMissionValid();}; /** - * @brief Go to next Mission Item + * @brief Go to next Mission Item using this mode's traversal policy. * Go to next non jump mission item * - * @param[in] execute_jump Flag indicating if a jump should be executed or ignored * @return PX4_OK if next mission item exists, PX4_ERR otherwise */ - int goToNextItem(bool execute_jump); + int goToNextItem(); /** - * @brief Go to previous Mission Item + * @brief Go to next Mission Item using an explicit traversal policy. + * Go to next non jump mission item + * + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextItem(MissionTraversalType traversal_type); + /** + * @brief Go to previous Mission Item using this mode's traversal policy. * Go to previous non jump mission item - * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * * @return PX4_OK if previous mission item exists, PX4_ERR otherwise */ - int goToPreviousItem(bool execute_jump); + int goToPreviousItem(); + /** + * @brief Go to previous Mission Item using an explicit traversal policy. + * Go to previous non jump mission item + * + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousItem(MissionTraversalType traversal_type); /** * @brief Go to Mission Item * * @param[in] index Index of the mission item to go to - * @param[in] execute_jump Flag indicating if a jump should be executed of ignored + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items * @param[in] mission_direction_backward Flag indicating if a mission is flown backward * @return PX4_OK if the mission item exists, PX4_ERR otherwise */ - int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false); + int goToItem(int32_t index, MissionTraversalType traversal_type = MissionTraversalType::FollowMissionControlFlow, + bool mission_direction_backward = false); /** - * @brief Go To Previous Mission Position Item + * @brief Go To Previous Mission Position Item using this mode's traversal policy. * - * @param[in] execute_jump Flag indicating if a jump should be executed or ignored * @return PX4_OK if previous mission item exists, PX4_ERR otherwise */ - int goToPreviousPositionItem(bool execute_jump); + int goToPreviousPositionItem(); /** - * @brief Go To Next Mission Position Item + * @brief Go To Previous Mission Position Item using an explicit traversal policy. + * + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousPositionItem(MissionTraversalType traversal_type); + /** + * @brief Go To Next Mission Position Item using this mode's traversal policy. * - * @param[in] execute_jump Flag indicating if a jump should be executed or ignored * @return PX4_OK if next mission item exists, PX4_ERR otherwise */ - int goToNextPositionItem(bool execute_jump); + int goToNextPositionItem(); + /** + * @brief Go To Next Mission Position Item using an explicit traversal policy. + * + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextPositionItem(MissionTraversalType traversal_type); /** * @brief Go to Mission Land Start Item * @@ -202,13 +262,13 @@ protected: * * @param[out] mission_index Index of the mission item * @param[out] mission The return mission item - * @param execute_jump Flag indicating if a jump item should be executed or ignored + * @param traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items * @param write_jumps Flag indicating if the jump counter should be updated * @param mission_direction_backward Flag indicating if the mission is flown backwards * @return PX4_OK if mission item could be loaded, PX4_ERR otherwise */ - int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps, - bool mission_direction_backward = false); + int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, MissionTraversalType traversal_type, + bool write_jumps, bool mission_direction_backward = false); /** * @brief Is Mission Valid * @@ -316,6 +376,18 @@ protected: */ bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; + /** + * @brief Traversal mode used by this navigation mode when walking position items. + * + * Mission mode follows active DO_JUMP control flow by default. Derived modes such as + * mission-based RTL can override this to walk the geometric mission path instead. + * Traversal helpers use this policy unless the caller explicitly overrides it. + */ + virtual MissionTraversalType traversalType() const + { + return MissionTraversalType::FollowMissionControlFlow; + } + /** * @brief Set the Mission Index * @@ -323,6 +395,40 @@ protected: */ void setMissionIndex(int32_t index); + /** + * @brief Load a single mission item from the dataman cache. + * + * @param[in] index Index of the mission item + * @param[out] mission_item The loaded mission item + * @return true if the item was loaded successfully + */ + virtual bool loadMissionItemFromCache(int32_t index, mission_item_s &mission_item); + + /** + * @brief Find the next position mission item. + * + * Walks forward through the mission starting at @p start_index. + * + * @param[in] start_index First index to check + * @param[out] next_index Index of the found position item + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + * @return true if a position item was found + */ + bool findNextPositionIndex(int32_t start_index, int32_t &next_index, + MissionTraversalType traversal_type); + + /** + * @brief Find the previous position mission item. + * + * Walks backward through the mission starting at @p start_index - 1. + * + * @param[in] start_index Search starts one before this index + * @param[out] previous_index Index of the found position item + * @param[in] traversal_type Whether to follow DO_JUMP mission control flow or ignore DO_JUMP items + * @return true if a position item was found + */ + bool findPreviousPositionIndex(int32_t start_index, int32_t &previous_index, + MissionTraversalType traversal_type); bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ @@ -363,6 +469,12 @@ private: */ void updateMavlinkMission(); + /** + * @brief Load a mission item according to the requested mission traversal type. + */ + bool loadTraversalItem(int32_t &mission_index, mission_item_s &mission_item, + MissionTraversalType traversal_type, bool direction_backward); + /** * Reset mission */ diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 7fdbe1add5..7203c381fb 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -96,7 +96,7 @@ void RtlDirectMissionLand::on_activation() _needs_climbing = false; if (hasMissionLandStart()) { - _is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK); + _is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, MissionTraversalType::IgnoreDoJump) == PX4_OK); _needs_climbing = checkNeedsToClimb(); @@ -115,7 +115,7 @@ void RtlDirectMissionLand::on_activation() bool RtlDirectMissionLand::setNextMissionItem() { - return (goToNextPositionItem(true) == PX4_OK); + return (goToNextPositionItem() == PX4_OK); } void RtlDirectMissionLand::setActiveMissionItems() diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index 6c1d99d5fd..52b7fa07f9 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -101,7 +101,7 @@ void RtlMissionFast::on_activation() bool RtlMissionFast::setNextMissionItem() { - return (goToNextPositionItem(true) == PX4_OK); + return (goToNextPositionItem() == PX4_OK); } void RtlMissionFast::setActiveMissionItems() diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h index 38814f96d0..70993219c8 100644 --- a/src/modules/navigator/rtl_mission_fast.h +++ b/src/modules/navigator/rtl_mission_fast.h @@ -63,6 +63,7 @@ public: private: bool setNextMissionItem() override; void setActiveMissionItems() override; + MissionTraversalType traversalType() const override { return MissionTraversalType::IgnoreDoJump; } int32_t _mission_index_prior_rtl{INT32_C(-1)}; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index f3d8c7b199..af85fd828d 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -87,7 +87,7 @@ void RtlMissionFastReverse::on_activation() } else { // No prior position items, so try to go to the first one. - _is_current_planned_mission_item_valid = (goToNextPositionItem(false) == PX4_OK); + _is_current_planned_mission_item_valid = (goToNextPositionItem() == PX4_OK); } } @@ -107,7 +107,7 @@ void RtlMissionFastReverse::on_active() bool RtlMissionFastReverse::setNextMissionItem() { - return (goToPreviousPositionItem(true) == PX4_OK); + return (goToPreviousPositionItem() == PX4_OK); } void RtlMissionFastReverse::setActiveMissionItems() diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index 8f96b95dc6..e75ba0e19e 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -67,6 +67,7 @@ public: private: bool setNextMissionItem() override; void setActiveMissionItems() override; + MissionTraversalType traversalType() const override { return MissionTraversalType::IgnoreDoJump; } void handleLanding(WorkItemType &new_work_item_type); int32_t _mission_index_prior_rtl{INT32_C(-1)}; diff --git a/src/modules/navigator/rtl_params.yaml b/src/modules/navigator/rtl_params.yaml index 973f65043a..30c09f65bc 100644 --- a/src/modules/navigator/rtl_params.yaml +++ b/src/modules/navigator/rtl_params.yaml @@ -66,14 +66,17 @@ parameters: rally point), via direct path. If no mission landing or rally points are defined return home via direct path. Always choose closest safe landing point if vehicle is a VTOL in hover mode. - 2: Return to a planned mission landing, if available, using the mission path, - else return to home via the reverse mission path. Do not consider rally - points. + 2: Return to a planned mission landing, if available, using the mission + path while skipping DO_JUMP and other non-position mission items, else + return to home via the reverse mission path with the same traversal + rules. Do not consider rally points. 3: 'Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land.' 4: Return to the planned mission landing, or to home via the reverse mission - path, whichever is closer by counting waypoints. Do not consider rally points. + path, whichever is estimated to be closer using mission item indices. + Skip DO_JUMP and other non-position mission items while following either + mission path. Do not consider rally points. 5: Return directly to safe landing point (do not consider mission landing and Home) default: 0 diff --git a/src/modules/navigator/test/CMakeLists.txt b/src/modules/navigator/test/CMakeLists.txt new file mode 100644 index 0000000000..a1e678f018 --- /dev/null +++ b/src/modules/navigator/test/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2026 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/..) + +px4_add_functional_gtest(SRC test_mission_base.cpp LINKLIBS modules__navigator) diff --git a/src/modules/navigator/test/test_mission_base.cpp b/src/modules/navigator/test/test_mission_base.cpp new file mode 100644 index 0000000000..6313d32e4a --- /dev/null +++ b/src/modules/navigator/test/test_mission_base.cpp @@ -0,0 +1,770 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_mission_base.cpp + * + * Unit tests for MissionBase position traversal helpers: + * - getNonJumpItem() + * - findNextPositionIndex() + * - findPreviousPositionIndex() + * - getNextPositionItems() + * - getPreviousPositionItems() + * - goToNextPositionItem() + * - goToPreviousPositionItem() + * + * These tests cover both traversal modes: Follow mission control flow and ignore DO_JUMP. + * + * @author Jonas Perolini + * + */ + +#include + +#include "mission_base.h" + +#include +#include + +#include +#include + +extern "C" int dataman_main(int argc, char *argv[]); + +class NavigatorDatamanRuntime +{ +public: + NavigatorDatamanRuntime() + { + param_control_autosave(false); + px4::WorkQueueManagerStart(); + + char name[] = "dataman"; + char start[] = "start"; + char ram[] = "-r"; + char *argv[] = {name, start, ram}; + dataman_main(3, argv); + } + + ~NavigatorDatamanRuntime() + { + param_control_autosave(true); + + char name[] = "dataman"; + char stop[] = "stop"; + char *argv[] = {name, stop}; + dataman_main(2, argv); + + px4::WorkQueueManagerStop(); + } +}; + +static NavigatorDatamanRuntime &navigatorDatamanRuntime() +{ + static NavigatorDatamanRuntime runtime{}; + return runtime; +} + +class MissionBaseTestPeer : public MissionBase +{ +public: + MissionBaseTestPeer() : MissionBase(nullptr, 8, 0) {} + + void setActiveMissionItems() override {} + bool setNextMissionItem() override { return false; } + + bool loadMissionItemFromCache(int32_t index, mission_item_s &mission_item) override + { + for (int32_t failed_index : _load_failure_indices) { + if (failed_index == index) { + return false; + } + } + + if (index < 0 || index >= static_cast(_items.size())) { + return false; + } + + mission_item = _items[static_cast(index)]; + return true; + } + + void loadTestMission(const std::vector &items) + { + _items = items; + _mission.count = static_cast(items.size()); + _mission.current_seq = 0; + clearLoadFailures(); + } + + void setLoadFailureIndices(std::initializer_list indices) + { + _load_failure_indices.assign(indices.begin(), indices.end()); + } + + void clearLoadFailures() + { + _load_failure_indices.clear(); + } + + void setCurrentSequence(int32_t current_seq) + { + _mission.current_seq = current_seq; + } + + int32_t currentSequence() const + { + return _mission.current_seq; + } + + using MissionBase::findNextPositionIndex; + using MissionBase::findPreviousPositionIndex; + using MissionBase::getNonJumpItem; + using MissionBase::getNextPositionItems; + using MissionBase::getPreviousPositionItems; + using MissionBase::goToNextPositionItem; + using MissionBase::goToPreviousPositionItem; + using MissionBase::MissionTraversalType; + +private: + std::vector _items; + std::vector _load_failure_indices; +}; + +class IgnoreDoJumpMissionBaseTestPeer : public MissionBaseTestPeer +{ +protected: + MissionTraversalType traversalType() const override + { + return MissionTraversalType::IgnoreDoJump; + } +}; + +static constexpr double kBaseLat = 47.0; +static constexpr double kBaseLon = 8.0; +static constexpr float kAlt = 100.f; + +static mission_item_s makePositionItem(double lat, double lon, float altitude) +{ + mission_item_s item{}; + item.nav_cmd = NAV_CMD_WAYPOINT; + item.lat = lat; + item.lon = lon; + item.altitude = altitude; + return item; +} + +static mission_item_s makeDoJump(int32_t target_index, uint16_t repeat_count, uint16_t current_count = 0) +{ + mission_item_s item{}; + item.nav_cmd = NAV_CMD_DO_JUMP; + item.do_jump_mission_index = target_index; + item.do_jump_repeat_count = repeat_count; + item.do_jump_current_count = current_count; + return item; +} + +static mission_item_s makeVtolTransitionItem(int transition_mode) +{ + mission_item_s item{}; + item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; + item.params[0] = static_cast(transition_mode); + return item; +} + +class MissionBaseTraversalTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + (void)navigatorDatamanRuntime(); + } + + static void TearDownTestSuite() {} + + MissionBaseTestPeer mission_base{}; +}; + +class IgnoreDoJumpMissionBaseTraversalTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + (void)navigatorDatamanRuntime(); + } + + static void TearDownTestSuite() {} + + IgnoreDoJumpMissionBaseTestPeer mission_base{}; +}; + +// WHY: getNonJumpItem is used to find the next mission item. +// WHAT: A non-DO_JUMP item is returned unchanged. +TEST_F(MissionBaseTraversalTest, GetNonJumpItemReturnsCurrentNonJumpItem) +{ + // GIVEN: A mission that starts with a normal position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 1, 0), // idx 1 + }); + + int32_t mission_index = 0; + mission_item_s mission_item{}; + + // WHEN: The helper loads the current item directly. + const int ret = mission_base.getNonJumpItem(mission_index, mission_item, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow, + false, false); + + // THEN: It returns the same item and leaves the index unchanged. + ASSERT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_index, 0); + EXPECT_EQ(mission_item.nav_cmd, NAV_CMD_WAYPOINT); + EXPECT_DOUBLE_EQ(mission_item.lat, kBaseLat); + EXPECT_DOUBLE_EQ(mission_item.lon, kBaseLon); + EXPECT_FLOAT_EQ(mission_item.altitude, kAlt); +} + +// WHY: getNonJumpItem() must follow active DO_JUMP targets. +// WHAT: [DO_JUMP->2, WP1, WP2] starting from idx 0 returns idx 2. +TEST_F(MissionBaseTraversalTest, GetNonJumpItemFollowsActiveForwardDoJump) +{ + // GIVEN: A forward DO_JUMP that points to a later position item. + mission_base.loadTestMission({ + makeDoJump(2, 1, 0), // idx 0 + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + + int32_t mission_index = 0; + mission_item_s mission_item{}; + + // WHEN: Mission-control traversal resolves the jump without writing counters. + const int ret = mission_base.getNonJumpItem(mission_index, mission_item, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow, + false, false); + + // THEN: The helper follows the jump and returns the target item. + ASSERT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_index, 2); + EXPECT_EQ(mission_item.nav_cmd, NAV_CMD_WAYPOINT); + EXPECT_DOUBLE_EQ(mission_item.lat, kBaseLat + 0.001); + EXPECT_DOUBLE_EQ(mission_item.lon, kBaseLon); + EXPECT_FLOAT_EQ(mission_item.altitude, kAlt); +} + +// WHY: Once a DO_JUMP has already used all repeats, callers should move on to the next item. +// WHAT: [WP0, DO_JUMP->0 done, WP2] starting from idx 1 returns idx 2. +TEST_F(MissionBaseTraversalTest, GetNonJumpItemSkipsDoJumpAfterLastRepeat) +{ + // GIVEN: A DO_JUMP whose repeat count is already reached. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 1, 1), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + + int32_t mission_index = 1; + mission_item_s mission_item{}; + + // WHEN: The helper resolves the jump while traversing forward. + const int ret = mission_base.getNonJumpItem(mission_index, mission_item, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow, + false, false); + + // THEN: The jump is skipped and the next non-jump item is returned. + ASSERT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_index, 2); + EXPECT_EQ(mission_item.nav_cmd, NAV_CMD_WAYPOINT); + EXPECT_DOUBLE_EQ(mission_item.lat, kBaseLat + 0.001); +} + +// WHY: Reverse traversal that ignores DO_JUMP must step backward instead of following control flow. +// WHAT: [WP0, DO_JUMP->2, WP2] starting from idx 1 returns idx 0. +TEST_F(MissionBaseTraversalTest, GetNonJumpItemSkipsDoJumpBackwardWhenIgnoringJumps) +{ + // GIVEN: An active forward DO_JUMP with a valid non-jump item before it. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(2, 1, 0), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + + int32_t mission_index = 1; + mission_item_s mission_item{}; + + // WHEN: The helper resolves the jump while moving backward in geometry-only mode. + const int ret = mission_base.getNonJumpItem(mission_index, mission_item, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump, + false, true); + + // THEN: The DO_JUMP is skipped and the previous non-jump item is returned. + ASSERT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_index, 0); + EXPECT_EQ(mission_item.nav_cmd, NAV_CMD_WAYPOINT); + EXPECT_DOUBLE_EQ(mission_item.lat, kBaseLat); +} + +// WHY: Bad jump targets must return an error. +// WHAT: A DO_JUMP that points beyond the mission bounds returns PX4_ERROR. +TEST_F(MissionBaseTraversalTest, GetNonJumpItemReturnsErrorForOutOfBoundsDoJumpTarget) +{ + // GIVEN: A mission with a DO_JUMP that points outside the mission. + mission_base.loadTestMission({ + makeDoJump(3, 1, 0), // idx 0 + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 1 + }); + + int32_t mission_index = 0; + mission_item_s mission_item{}; + + // WHEN: The helper tries to resolve that jump. + const int ret = mission_base.getNonJumpItem(mission_index, mission_item, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow, + false, false); + + // THEN: The helper returns an error. + EXPECT_EQ(ret, PX4_ERROR); + EXPECT_EQ(mission_index, 0); +} + +// WHY: Geometry-only position traversal must skip non-position mission items. +// WHAT: Starting from a VTOL transition item, the helper skips it and returns the next position item. +TEST_F(MissionBaseTraversalTest, FindNextSkipsNonPositionItems) +{ + // GIVEN: A position item, a non-position VTOL transition, and then another position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeVtolTransitionItem(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + + int32_t next_index{-1}; + + // WHEN: Geometry-only traversal searches forward from the non-position item. + const bool found = mission_base.findNextPositionIndex(1, next_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The next position item is returned. + EXPECT_TRUE(found); + EXPECT_EQ(next_index, 2); +} + +// WHY: Geometry-only position traversal must skip non-position mission items. +// WHAT: Starting from a position item after a VTOL transition, the helper skips it and returns the previous position item. +TEST_F(MissionBaseTraversalTest, FindPreviousSkipsNonPositionItems) +{ + // GIVEN: A position item, a non-position VTOL transition, and then another position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeVtolTransitionItem(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + + int32_t previous_index{-1}; + + // WHEN: Geometry-only traversal searches backward from the non-position item. + const bool found = mission_base.findPreviousPositionIndex(2, previous_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The previous position item is returned. + EXPECT_TRUE(found); + EXPECT_EQ(previous_index, 0); +} + +// WHY: Geometry-only traversal must skip DO_JUMP items. +// WHAT: [WP, DO_JUMP, WP, WP] starting from idx 1 returns idx 2. +TEST_F(MissionBaseTraversalTest, FindNextSkipsDoJumpItems) +{ + // GIVEN: A mission where a DO_JUMP sits between two position items. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 3), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + makePositionItem(kBaseLat + 0.002, kBaseLon, kAlt), // idx 3 + }); + + int32_t next_index{-1}; + + // WHEN: Geometry-only traversal starts at the DO_JUMP item. + const bool found = mission_base.findNextPositionIndex(1, next_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The first position item after the jump is returned. + EXPECT_TRUE(found); + EXPECT_EQ(next_index, 2); +} + +// WHY: Geometry-only traversal must skip DO_JUMP items. +// WHAT: [WP, WP, DO_JUMP, WP] starting from idx 3 returns idx 1. +TEST_F(MissionBaseTraversalTest, FindPreviousSkipsDoJumpItems) +{ + // GIVEN: A mission where a DO_JUMP sits between two position items. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 1 + makeDoJump(0, 3), // idx 2 + makePositionItem(kBaseLat + 0.002, kBaseLon, kAlt), // idx 3 + }); + + int32_t previous_index{-1}; + + // WHEN: Geometry-only traversal starts at the DO_JUMP item. + const bool found = mission_base.findPreviousPositionIndex(3, previous_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The first position item before the jump is returned. + EXPECT_TRUE(found); + EXPECT_EQ(previous_index, 1); +} + +// WHY: Consecutive non-position items must be skipped. +// WHAT: [WP, DO_JUMP, VTOL_FW, WP] starting from idx 1 returns idx 3. +TEST_F(MissionBaseTraversalTest, FindNextSkipsConsecutiveNonPositionItems) +{ + // GIVEN: Consecutive non-position items before the next position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 5), // idx 1 + makeVtolTransitionItem(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW), // idx 2 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 3 + }); + + int32_t next_index{-1}; + + // WHEN: Geometry-only traversal walks forward through the control items. + const bool found = mission_base.findNextPositionIndex(1, next_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: It returns the first following position item. + EXPECT_TRUE(found); + EXPECT_EQ(next_index, 3); +} + +// WHY: Consecutive non-position items must be skipped in reverse. +// WHAT: [WP, DO_JUMP, VTOL_FW, WP] starting from idx 3 returns idx 0. +TEST_F(MissionBaseTraversalTest, FindPreviousSkipsConsecutiveNonPositionItems) +{ + // GIVEN: Consecutive non-position items before the previous position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 3), // idx 1 + makeVtolTransitionItem(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW), // idx 2 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 3 + }); + + int32_t previous_index{-1}; + + // WHEN: Geometry-only traversal walks backward through the control items. + const bool found = mission_base.findPreviousPositionIndex(3, previous_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: It returns the first previous position item. + EXPECT_TRUE(found); + EXPECT_EQ(previous_index, 0); +} + +// WHY: Callers need a failure when no later position item exists. +// WHAT: [WP, DO_JUMP] starting from idx 1 returns false. +TEST_F(MissionBaseTraversalTest, FindNextReturnsFalseAtEnd) +{ + // GIVEN: A mission with no position item after the starting index. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 3), // idx 1 + }); + + int32_t next_index{-1}; + + // WHEN: Geometry-only traversal searches past the last item. + const bool found = mission_base.findNextPositionIndex(1, next_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The helper reports that no next position item exists. + EXPECT_FALSE(found); + EXPECT_EQ(next_index, -1); +} + +// WHY: Callers need a failure when no earlier position item exists. +// WHAT: [DO_JUMP, WP] starting from idx 1 returns false. +TEST_F(MissionBaseTraversalTest, FindPreviousReturnsFalseAtStart) +{ + // GIVEN: A mission with no position item before the starting index. + mission_base.loadTestMission({ + makeDoJump(0, 3), // idx 0 + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 1 + }); + + int32_t previous_index{-1}; + + // WHEN: Geometry-only traversal searches before the first position item. + const bool found = mission_base.findPreviousPositionIndex(1, previous_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The helper reports that no previous position item exists. + EXPECT_FALSE(found); + EXPECT_EQ(previous_index, -1); +} + +// WHY: Traversal should fail cleanly when a mission item cannot be loaded. +// WHAT: A cache failure on the next position item makes findNextPositionIndex() return false. +TEST_F(MissionBaseTraversalTest, FindNextReturnsFalseOnCacheReadFailure) +{ + // GIVEN: A mission where the next position item cannot be loaded. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 3), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + mission_base.setLoadFailureIndices({2}); + + int32_t next_index{-1}; + + // WHEN: Geometry-only traversal advances past the DO_JUMP item. + const bool found = mission_base.findNextPositionIndex(1, next_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The unreadable position item produces a clean failure. + EXPECT_FALSE(found); + EXPECT_EQ(next_index, -1); +} + +// WHY: Traversal should fail cleanly when a mission item cannot be loaded. +// WHAT: A cache failure on the previous position item makes findPreviousPositionIndex() return false. +TEST_F(MissionBaseTraversalTest, FindPreviousReturnsFalseOnCacheReadFailure) +{ + // GIVEN: A mission where the previous position item cannot be loaded. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makeDoJump(0, 3), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + mission_base.setLoadFailureIndices({0}); + + int32_t previous_index{-1}; + + // WHEN: Geometry-only traversal moves backward past the DO_JUMP item. + const bool found = mission_base.findPreviousPositionIndex(2, previous_index, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: The unreadable position item produces a clean failure. + EXPECT_FALSE(found); + EXPECT_EQ(previous_index, -1); +} + +// WHY: findNextPositionIndex must use MissionTraversalType +// WHAT: [DO_JUMP->2, WP1, WP2] starting from idx 0 resolves to idx 2 in mission-control +// mode and idx 1 in geometry-only mode. +TEST_F(MissionBaseTraversalTest, FindNextSupportsBothTraversalSemantics) +{ + // GIVEN: A jump whose target is a later position item. + mission_base.loadTestMission({ + makeDoJump(2, 1, 0), // idx 0 + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + + int32_t next_follow{-1}; + int32_t next_geometry{-1}; + + // WHEN: The same lookup is performed in both traversal modes. + const bool found_follow = mission_base.findNextPositionIndex(0, next_follow, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow); + const bool found_geometry = mission_base.findNextPositionIndex(0, next_geometry, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: Mission-control mode follows the jump, while geometry-only mode skips it. + EXPECT_TRUE(found_follow); + EXPECT_TRUE(found_geometry); + EXPECT_EQ(next_follow, 2); + EXPECT_EQ(next_geometry, 1); +} + +// WHY: findPreviousPositionIndex must use MissionTraversalType +// WHAT: [WP0, WP1, DO_JUMP->0, WP3] starting from idx 3 resolves to idx 0 in mission-control +// mode and idx 1 in geometry-only mode. +TEST_F(MissionBaseTraversalTest, FindPreviousSupportsBothTraversalSemantics) +{ + // GIVEN: A jump whose target is an earlier position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 1 + makeDoJump(0, 2, 0), // idx 2 + makePositionItem(kBaseLat + 0.002, kBaseLon, kAlt), // idx 3 + }); + + int32_t previous_follow{-1}; + int32_t previous_geometry{-1}; + + // WHEN: The same lookup is performed in both traversal modes. + const bool found_follow = mission_base.findPreviousPositionIndex(3, previous_follow, + MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow); + const bool found_geometry = mission_base.findPreviousPositionIndex(3, previous_geometry, + MissionBaseTestPeer::MissionTraversalType::IgnoreDoJump); + + // THEN: Mission-control mode follows the jump, while geometry-only mode skips it. + EXPECT_TRUE(found_follow); + EXPECT_TRUE(found_geometry); + EXPECT_EQ(previous_follow, 0); + EXPECT_EQ(previous_geometry, 1); +} + +// WHY: The refactor must not change the legacy mission-control behavior. +// WHAT: [DO_JUMP->2, WP1, WP2] from current_seq=-1 should still land on idx 2. +TEST_F(MissionBaseTraversalTest, GoToNextPositionItemFollowsMissionControlFlow) +{ + // GIVEN: A mission whose first item is an active DO_JUMP. + mission_base.loadTestMission({ + makeDoJump(2, 1, 0), // idx 0 + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + mission_base.setCurrentSequence(-1); + + // WHEN: The caller requests mission-control traversal. + const int ret = mission_base.goToNextPositionItem(MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow); + + // THEN: Traversal follows the jump target exactly as before. + EXPECT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_base.currentSequence(), 2); +} + +// WHY: The backward wrapper must also preserve the legacy mission-control behavior. +// WHAT: [WP0, WP1, DO_JUMP->0, WP3] from current_seq=3 should still land on idx 0. +TEST_F(MissionBaseTraversalTest, GoToPreviousPositionItemFollowsMissionControlFlow) +{ + // GIVEN: A mission with an active jump loop before the current position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 1 + makeDoJump(0, 2, 0), // idx 2 + makePositionItem(kBaseLat + 0.002, kBaseLon, kAlt), // idx 3 + }); + mission_base.setCurrentSequence(3); + + // WHEN: The caller requests mission-control traversal. + const int ret = mission_base.goToPreviousPositionItem(MissionBaseTestPeer::MissionTraversalType::FollowMissionControlFlow); + + // THEN: Traversal follows the active jump exactly as before. + EXPECT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_base.currentSequence(), 0); +} + +// WHY: Existing mission execution relies on getNextPositionItems() following active DO_JUMP +// control flow by default. +// WHAT: [WP0, WP1, DO_JUMP->0, WP3] starting from idx 2 returns idx 0 then idx 1. +TEST_F(MissionBaseTraversalTest, GetNextPositionItemsFollowsActiveDoJump) +{ + // GIVEN: A mission with an active jump loop. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 1 + makeDoJump(0, 2, 0), // idx 2 + makePositionItem(kBaseLat + 0.002, kBaseLon, kAlt), // idx 3 + }); + + int32_t next_items[2] = {-1, -1}; + size_t num_found_items = 0; + + // WHEN: The multi-item helper walks forward with default traversal semantics. + mission_base.getNextPositionItems(2, next_items, num_found_items, 2u); + + // THEN: The active DO_JUMP is followed. + ASSERT_EQ(num_found_items, 2u); + EXPECT_EQ(next_items[0], 0); + EXPECT_EQ(next_items[1], 1); +} + +// WHY: Reverse mission flows rely on getPreviousPositionItems() following active DO_JUMP. +// WHAT: [WP0, WP1, DO_JUMP->0, WP3] starting from idx 3 returns idx 0. +TEST_F(MissionBaseTraversalTest, GetPreviousPositionItemsFollowsActiveDoJump) +{ + // GIVEN: A mission with an active jump loop before the current position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 1 + makeDoJump(0, 2, 0), // idx 2 + makePositionItem(kBaseLat + 0.002, kBaseLon, kAlt), // idx 3 + }); + + int32_t previous_items[1] = {-1}; + size_t num_found_items = 0; + + // WHEN: The multi-item helper walks backward with default traversal semantics. + mission_base.getPreviousPositionItems(3, previous_items, num_found_items, 1u); + + // THEN: The active DO_JUMP is followed. + ASSERT_EQ(num_found_items, 1u); + EXPECT_EQ(previous_items[0], 0); +} + +// WHY: Mission-based RTL configures position traversal to skip DO_JUMP loops consistently. +// WHAT: [DO_JUMP->2, WP1, WP2] from current_seq=-1 lands on idx 1 with the configured traversal. +TEST_F(IgnoreDoJumpMissionBaseTraversalTest, ConfiguredTraversalSkipsDoJumpForGoToNextPositionItem) +{ + // GIVEN: A mission whose first item is an active DO_JUMP. + mission_base.loadTestMission({ + makeDoJump(2, 1, 0), // idx 0 + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 1 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 2 + }); + mission_base.setCurrentSequence(-1); + + // WHEN: The mode advances using its configured traversal policy. + const int ret = mission_base.goToNextPositionItem(); + + // THEN: The DO_JUMP loop is skipped and the geometric next waypoint is selected. + EXPECT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_base.currentSequence(), 1); +} + +// WHY: Reverse mission-path RTL must skip DO_JUMP loops for backward progression too. +// WHAT: [WP0, WP1, DO_JUMP->0, WP3] from current_seq=3 lands on idx 1 with the configured traversal. +TEST_F(IgnoreDoJumpMissionBaseTraversalTest, ConfiguredTraversalSkipsDoJumpForGoToPreviousPositionItem) +{ + // GIVEN: A mission with an active jump loop before the current position item. + mission_base.loadTestMission({ + makePositionItem(kBaseLat, kBaseLon, kAlt), // idx 0 + makePositionItem(kBaseLat + 0.001, kBaseLon, kAlt), // idx 1 + makeDoJump(0, 2, 0), // idx 2 + makePositionItem(kBaseLat + 0.002, kBaseLon, kAlt), // idx 3 + }); + mission_base.setCurrentSequence(3); + + // WHEN: The mode advances backward using its configured traversal policy. + const int ret = mission_base.goToPreviousPositionItem(); + + // THEN: The DO_JUMP loop is skipped and the geometric previous waypoint is selected. + EXPECT_EQ(ret, PX4_OK); + EXPECT_EQ(mission_base.currentSequence(), 1); +}