From 92f7fb2732a25bcdfe1afa358c3a5a2715d53a1b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Nov 2013 14:17:41 +0100 Subject: Navigator: Added some mavlink debug output --- src/modules/navigator/navigator_main.cpp | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e52331bcf..4a239f901 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include typedef enum { NAVIGATION_MODE_NONE, @@ -132,6 +133,8 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _navigator_task; /**< task handle for sensor task */ + int _mavlink_fd; + int _global_pos_sub; /**< global position subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ @@ -248,6 +251,8 @@ Navigator::Navigator() : _task_should_exit(false), _navigator_task(-1), + _mavlink_fd(-1), + /* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), @@ -344,6 +349,7 @@ Navigator::mission_update() if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { /* set flag to restart mission next we're in auto */ _current_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); //warnx("First part of mission differs i=%d", i); break; } @@ -354,6 +360,7 @@ Navigator::mission_update() } else { /* set flag to restart mission next we're in auto */ _current_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); } /* @@ -370,6 +377,7 @@ Navigator::mission_update() } else { warnx("ERROR: too many waypoints, not supported"); + mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported"); _mission_item_count = 0; } @@ -550,6 +558,12 @@ Navigator::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + /* XXX Hack to get mavlink output going */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -566,7 +580,7 @@ Navigator::task_main() check_mission_item_reached(); if (_mission_item_reached) { - // warnx("Mission already reached"); + mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); if (advance_current_mission_item() != OK) { set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); } @@ -578,6 +592,7 @@ Navigator::task_main() check_mission_item_reached(); if (_mission_item_reached) { + mavlink_log_info(_mavlink_fd, "[navigator] reached RTL position"); set_mode(NAVIGATION_MODE_LOITER_RTL); } break; @@ -756,7 +771,7 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) global_position_mission_item.altitude = _global_pos.alt; start_loiter(&global_position_mission_item); _mode = new_nav_mode; - // warnx("start loiter here"); + mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); break; case NAVIGATION_MODE_LOITER_WAYPOINT: @@ -777,15 +792,15 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) /* Start mission if there is one available */ start_waypoint(); - // warnx("Set mode WAYPOINT"); _mode = new_nav_mode; + mavlink_log_info(_mavlink_fd, "[navigator] start waypoint mission"); break; case NAVIGATION_MODE_LOITER_WAYPOINT: start_loiter(&_mission_item_triplet.current); - // warnx("Set mode LOITER from current waypoint"); _mode = new_nav_mode; + mavlink_log_info(_mavlink_fd, "[navigator] loiter at WP %d", _current_mission_index-1); break; case NAVIGATION_MODE_RTL: @@ -800,12 +815,11 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) /* start RTL here */ start_rtl(); _mode = new_nav_mode; - // warnx("start RTL"); + mavlink_log_info(_mavlink_fd, "[navigator] start RTL"); break; case NAVIGATION_MODE_LOITER_RTL: /* already loitering after RTL, just continue */ - /* TODO: get rid of this conversion */ // warnx("stay in loiter after RTL"); break; @@ -824,7 +838,7 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) home_position_mission_item.lon = (double)_home_pos.lon / 1e7; home_position_mission_item.altitude = _home_pos.alt / 1e3f + 50.0f; start_loiter(&home_position_mission_item); - // warnx("Set mode LOITER from home position"); + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); _mode = new_nav_mode; break; } @@ -1022,6 +1036,8 @@ Navigator::start_waypoint() set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { // if the current mission is to loiter unlimited, don't bother about a next mission item -- cgit v1.2.3