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

add method to block fallback to mission

Browse Source 
failsafe navigation modes can use a flag in mission_result to tell the
commander to not switch back to mission
This commit is contained in:
Thomas Gubler
2014-07-23 22:58:19 +02:00
parent ed19faf428
commit 24f380137e
10 changed files with 53 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+7 -25
View File
@@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
_current_offboard_mission_index(-1),
_need_takeoff(true),
_takeoff(false),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false)
@@ -577,18 +575,18 @@ void
Mission::report_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.reached = true;
_mission_result.seq_reached = _current_offboard_mission_index;
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
}
publish_mission_result();
_navigator->publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
{
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_mission_result.seq_current = _current_offboard_mission_index;
publish_mission_result();
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->publish_mission_result();
save_offboard_mission_state();
}
@@ -596,23 +594,7 @@ Mission::report_current_offboard_mission_item()
void
Mission::report_mission_finished()
{
_mission_result.finished = true;
publish_mission_result();
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.reached = false;
_mission_result.finished = false;
}