diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-23 22:58:19 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-23 22:58:19 +0200 |
commit | 24f380137ecb91fb9647e22e1d29c13da5fc0357 (patch) | |
tree | 8202bd0cad2410fd04d550897d9c994bd6d0bc3d /src/modules/navigator/navigator_main.cpp | |
parent | ed19faf4289eed5eeb3339e9609e976b9195020c (diff) | |
download | px4-firmware-24f380137ecb91fb9647e22e1d29c13da5fc0357.tar.gz px4-firmware-24f380137ecb91fb9647e22e1d29c13da5fc0357.tar.bz2 px4-firmware-24f380137ecb91fb9647e22e1d29c13da5fc0357.zip |
add method to block fallback to mission
failsafe navigation modes can use a flag in mission_result to tell the
commander to not switch back to mission
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 22 |
1 files changed, 22 insertions, 0 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1ce6770c9..77124e8f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -108,6 +108,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _param_update_sub(-1), _pos_sp_triplet_pub(-1), + _mission_result_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -115,6 +116,7 @@ Navigator::Navigator() : _mission_item{}, _nav_caps{}, _pos_sp_triplet{}, + _mission_result{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -370,6 +372,9 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + /* Some failsafe modes prohibit the fallback to mission + * usually this is done after some time to make sure + * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -553,3 +558,20 @@ int navigator_main(int argc, char *argv[]) return 0; } + +void +Navigator::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; +} |