yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

finish moving to waypoint after a back transition

Browse Source
This commit is contained in:
Andreas Antener
2016-01-28 14:20:30 +01:00
parent 0bdfb465cf
commit dfb31f2252
2 changed files with 27 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -501,6 +501,31 @@ Mission::set_mission_items()
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
/* don't advance mission after FW to MC command */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type != WORK_ITEM_TYPE_CMD_BEFORE_MOVE
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& pos_sp_triplet->previous.valid) {
new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
}
/* after FW to MC transition finish moving to the waypoint */
if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE
&& pos_sp_triplet->previous.valid) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = pos_sp_triplet->previous.lat;
_mission_item.lon = pos_sp_triplet->previous.lon;
_mission_item.altitude = pos_sp_triplet->previous.alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
}
/*********************************** set setpoints and check next *********************************************/

3
src/modules/navigator/mission.h
View File

@@ -233,7 +233,8 @@ private:
WORK_ITEM_TYPE_DEFAULT, /**< default mission item */
WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */
WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */
WORK_ITEM_TYPE_ALIGN /**< align for next waypoint */
WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */
WORK_ITEM_TYPE_CMD_BEFORE_MOVE /**< */
} _work_item_type; /**< current type of work to do (sub mission item) */
};