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

Merge branch 'navigator_new' into navigator_new_vector

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-31 15:45:04 +04:00
parent ba7cbe48f2 f6ee354c10
commit 5c84479ce9
1 changed files with 24 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+24 -20
View File
@@ -497,11 +497,14 @@ Navigator::task_main_trampoline(int argc, char *argv[])
void void
Navigator::task_main() Navigator::task_main()
{ {
/* inform about start */ /* inform about start */
warnx("Initializing.."); warnx("Initializing..");
fflush(stdout); fflush(stdout);
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[navigator] started");
_fence_valid = load_fence(GEOFENCE_MAX_VERTICES); _fence_valid = load_fence(GEOFENCE_MAX_VERTICES);
/* /*
@@ -530,7 +533,7 @@ Navigator::task_main()
unsigned prevState = NAV_STATE_NONE; unsigned prevState = NAV_STATE_NONE;
bool pub_control_mode = true; bool pub_control_mode = true;
hrt_abstime mavlink_open_time = 0; hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_period = 500000000; const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */ /* wakeup source(s) */
struct pollfd fds[7]; struct pollfd fds[7];
@@ -569,9 +572,9 @@ Navigator::task_main()
perf_begin(_loop_perf); perf_begin(_loop_perf);
if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time + mavlink_open_period) { if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
/* try to open the mavlink log device every once in a while */ /* try to reopen the mavlink log device with specified interval */
mavlink_open_time = hrt_abstime(); mavlink_open_time = hrt_abstime() + mavlink_open_interval;
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
} }
@@ -591,19 +594,17 @@ Navigator::task_main()
stick_mode = true; stick_mode = true;
} else { } else {
/* MISSION switch */ /* 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); 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) { 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 */ /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
@@ -628,7 +629,11 @@ Navigator::task_main()
break; break;
case NAV_STATE_MISSION: case NAV_STATE_MISSION:
dispatch(EVENT_MISSION_REQUESTED); if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break; break;
case NAV_STATE_RTL: case NAV_STATE_RTL:
@@ -994,8 +999,7 @@ Navigator::start_loiter()
void void
Navigator::start_mission() Navigator::start_mission()
{ {
/* leave previous mission item as is as is */ /* leave previous mission item as is */
int ret; int ret;
bool onboard; bool onboard;
unsigned index; 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.yaw = 0.0f; // TODO use heading to waypoint?
_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
_mission_item_triplet.current.loiter_direction = 1; _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.radius = 50.0f; // TODO: get rid of magic number
_mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current.autocontinue = false;
_mission_item_triplet.current_valid = true; _mission_item_triplet.current_valid = true;