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

navigator: do not restart mission (continue with changed second part of mission) if no items prior to the current item have been changed

Browse Source
This commit is contained in:
Thomas Gubler
2013-11-26 01:19:37 +01:00
parent 8fb22a1df7
commit 5428eab23b
1 changed files with 49 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+49 -3
View File
@@ -221,6 +221,14 @@ private:
void start_loiter(mission_item_s *new_loiter_position);
void start_rtl();
/**
* Compare two mission items if they are equivalent
* Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ.
*
* @return true if equivalent, false otherwise
*/
bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b);
};
namespace navigator
@@ -327,6 +335,27 @@ Navigator::mission_update()
if (mission.count <= _max_mission_item_count) {
/* Check if first part of mission (up to _current_mission_index - 1) changed:
* if the first part changed: start again at first waypoint
* if the first part remained unchanged: continue with the (possibly changed second part)
*/
if (_current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission
for (int i = 0; i < (int)_current_mission_index; i++) {
if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) {
/* set flag to restart mission next we're in auto */
_current_mission_index = 0;
//warnx("First part of mission differs i=%d", i);
break;
}
// else {
// warnx("Mission item is equivalent i=%d", i);
// }
}
} else {
/* set flag to restart mission next we're in auto */
_current_mission_index = 0;
}
/*
* Perform an atomic copy & state update
*/
@@ -344,9 +373,6 @@ Navigator::mission_update()
_mission_item_count = 0;
}
/* set flag to restart mission next we're in auto */
_current_mission_index = 0;
if (_mode == NAVIGATION_MODE_WAYPOINT) {
start_waypoint();
}
@@ -1115,3 +1141,23 @@ int navigator_main(int argc, char *argv[])
return 0;
}
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
if (a.altitude_is_relative == b.altitude_is_relative &&
a.lat == b.lat &&
a.lon == b.lon &&
a.altitude == b.altitude &&
a.yaw == b.yaw &&
a.loiter_radius == b.loiter_radius &&
a.loiter_direction == b.loiter_direction &&
a.nav_cmd == b.nav_cmd &&
a.radius == b.radius &&
a.time_inside == b.time_inside &&
a.autocontinue == b.autocontinue &&
a.index == b.index) {
return true;
} else {
warnx("a.index %d, b.index %d", a.index, b.index);
return false;
}
}