diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-31 15:45:04 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-31 15:45:04 +0400 |
commit | 5c84479ce93792d7fa7b777387be3bf8d50cebae (patch) | |
tree | 035fb7c6b3aa0c656ec58a3e263f5b75eed47229 /src/modules | |
parent | ba7cbe48f2128f73d75e669432fa6c4874422d69 (diff) | |
parent | f6ee354c10026488c937568033a640d72a07cd14 (diff) | |
download | px4-firmware-5c84479ce93792d7fa7b777387be3bf8d50cebae.tar.gz px4-firmware-5c84479ce93792d7fa7b777387be3bf8d50cebae.tar.bz2 px4-firmware-5c84479ce93792d7fa7b777387be3bf8d50cebae.zip |
Merge branch 'navigator_new' into navigator_new_vector
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 44 |
1 files changed, 24 insertions, 20 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 428887373..34dcf8904 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -497,11 +497,14 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - /* inform about start */ warnx("Initializing.."); fflush(stdout); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + mavlink_log_info(_mavlink_fd, "[navigator] started"); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); /* @@ -530,7 +533,7 @@ Navigator::task_main() unsigned prevState = NAV_STATE_NONE; bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; - const hrt_abstime mavlink_open_period = 500000000; + const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ struct pollfd fds[7]; @@ -569,9 +572,9 @@ Navigator::task_main() perf_begin(_loop_perf); - if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time + mavlink_open_period) { - /* try to open the mavlink log device every once in a while */ - mavlink_open_time = hrt_abstime(); + if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { + /* try to reopen the mavlink log device with specified interval */ + mavlink_open_time = hrt_abstime() + mavlink_open_interval; _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } @@ -591,19 +594,17 @@ Navigator::task_main() stick_mode = true; } else { /* MISSION switch */ - if (!stick_mode) { - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - stick_mode = true; } + stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ @@ -628,7 +629,11 @@ Navigator::task_main() break; case NAV_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } break; case NAV_STATE_RTL: @@ -994,8 +999,7 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as is as is */ - + /* leave previous mission item as is */ int ret; bool onboard; unsigned index; @@ -1107,7 +1111,7 @@ Navigator::start_rtl() _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; |