diff options
author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-07-08 19:51:19 +0400 |
---|---|---|
committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-07-08 19:51:19 +0400 |
commit | 56649bd10a414a91237dd11142b33851b2a9015c (patch) | |
tree | aab55ec53ce8013d8e482eba9a9d75c9cc476446 /src/modules/navigator/navigator_main.cpp | |
parent | 664795c9db9a0d938cbe7221aed87755ca8de7bf (diff) | |
parent | 0054eb23d857844ebae34a7d198fc60e538ccd3c (diff) | |
download | px4-firmware-56649bd10a414a91237dd11142b33851b2a9015c.tar.gz px4-firmware-56649bd10a414a91237dd11142b33851b2a9015c.tar.bz2 px4-firmware-56649bd10a414a91237dd11142b33851b2a9015c.zip |
Merge branch 'master' into uavcan
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 22 |
1 files changed, 13 insertions, 9 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 546602741..1a5ba4c1a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,6 +67,7 @@ #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> +#include <uORB/topics/offboard_control_setpoint.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> @@ -100,6 +101,7 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), + _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -121,7 +123,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false), + _offboard(this, "OFF"), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD") { @@ -129,6 +131,7 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; + _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -241,6 +244,7 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -363,6 +367,9 @@ Navigator::task_main() break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: + _navigation_mode = &_offboard; + break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; @@ -371,24 +378,21 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - if (_navigation_mode == _navigation_mode_array[i]) { - _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); - } else { - _navigation_mode_array[i]->on_inactive(); - } + _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid */ if (_navigation_mode == nullptr) { + // TODO publish empty sp only once _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _update_triplet = true; + _pos_sp_triplet_updated = true; } - if (_update_triplet) { + if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); - _update_triplet = false; + _pos_sp_triplet_updated = false; } perf_end(_loop_perf); |