fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 20:49:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: Improve code readibility

Browse Source 
This diff does not contain any functional changes but just makes the code more readable and adds comments.
This commit is contained in:
Lorenz Meier 2020-03-07 17:06:15 +01:00
parent 417d172a72
commit 5e7ff47aac
2 changed files with 42 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/modules/navigator/mission.cpp
View File

@ -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, &current_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;
}
}

6
src/modules/navigator/mission_block.h
View File

@ -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);