From 5e7ff47aac0588db437245c4c695dae3da35bd19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Mar 2020 17:06:15 +0100 Subject: [PATCH] Navigator: Improve code readibility This diff does not contain any functional changes but just makes the code more readable and adds comments. --- src/modules/navigator/mission.cpp | 52 ++++++++++++++++++++------- src/modules/navigator/mission_block.h | 6 ++-- 2 files changed, 42 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e09d5948c2..47b50ec140 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -512,7 +512,7 @@ Mission::update_mission() // only warn if the check failed on merit if ((int)_mission.count > 0) { PX4_WARN("mission check failed"); - } + } // reset the mission _mission.count = 0; @@ -592,14 +592,14 @@ Mission::set_mission_items() /* mission item that comes after current if available */ struct mission_item_s mission_item_next_position; - struct mission_item_s mission_item_next_next_position; + struct mission_item_s mission_item_after_next_position; bool has_next_position_item = false; - bool has_next_next_position_item = false; + bool has_after_next_position_item = false; work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, - &mission_item_next_next_position, &has_next_next_position_item)) { + &mission_item_after_next_position, &has_after_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_MISSION) { mavlink_log_info(_navigator->get_mavlink_log_pub(), @@ -1058,21 +1058,43 @@ Mission::set_mission_items() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + // The logic in this section establishes the tracking between the current waypoint + // which we are approaching and the next waypoint, which will tell us in which direction + // we will change our trajectory right after reaching it. + + // Because actions, gates and jump labels can be interleaved with waypoints, + // we are searching around the current mission item in the list to find the closest + // gate and the closest waypoint. We then store them separately. + + // Check if the mission item is a gate + // along the current trajectory if (item_contains_gate(_mission_item)) { + + // The mission item is a gate, let's check if the next item in the list provides + // a position to go towards. + + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 if (has_next_position_item) { - /* got next mission item, update setpoint triplet */ + // We have a position, convert it to the setpoint and update setpoint triplet + mission_apply_limitation(mission_item_next_position); mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); } + // ELSE: The current position setpoint stays unchanged. + } else { - /* set current position setpoint from mission item (is protected against non-position items) */ + // The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items) + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } + + // ELSE: The current position setpoint stays unchanged. } - /* only set the previous position item if the current one really changed */ + // Only set the previous position item if the current one really changed + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) && !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { pos_sp_triplet->previous = current_setpoint_copy; @@ -1098,8 +1120,12 @@ Mission::set_mission_items() set_current_mission_item(); } + // If the mission item under evaluation contains a gate, + // then we need to check if we have a next position item so + // the controller can fly the correct line between the + // current and next setpoint if (item_contains_gate(_mission_item)) { - if (has_next_next_position_item) { + if (has_after_next_position_item) { /* got next mission item, update setpoint triplet */ mission_apply_limitation(mission_item_next_position); mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); @@ -1470,7 +1496,7 @@ Mission::do_abort_landing() bool Mission::prepare_mission_items(struct mission_item_s *mission_item, struct mission_item_s *next_position_mission_item, bool *has_next_position_item, - struct mission_item_s *next_next_position_mission_item, bool *has_next_next_position_item) + struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item) { *has_next_position_item = false; bool first_res = false; @@ -1500,13 +1526,13 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item, } if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE && - next_next_position_mission_item && has_next_next_position_item) { + after_next_position_mission_item && has_after_next_position_item) { /* trying to find next next position mission item */ - while (read_mission_item(offset, next_next_position_mission_item)) { + while (read_mission_item(offset, after_next_position_mission_item)) { offset++; - if (item_contains_position(*next_next_position_mission_item)) { - *has_next_next_position_item = true; + if (item_contains_position(*after_next_position_mission_item)) { + *has_after_next_position_item = true; break; } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index a6fc7dcda2..0e504230f5 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -70,21 +70,21 @@ public: /** * Check if the mission item contains a navigation position * - * @return false if mission item should not be part of the trajectory + * @return false if the mission item does not contain a valid position */ static bool item_contains_position(const mission_item_s &item); /** * Check if the mission item contains a gate condition * - * @return true if mission item is neither a position nor a command + * @return true if mission item is a gate */ static bool item_contains_gate(const mission_item_s &item); /** * Check if the mission item contains a marker * - * @return true if mission item is neither a position nor a command + * @return true if mission item is a marker */ static bool item_contains_marker(const mission_item_s &item);