dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: fix verbose mode and actually publish mission

This commit is contained in:
Julian Oes 2014-02-23 13:42:15 +01:00
parent 3ae35d8c18
commit 73edc02016
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/mavlink/mavlink_main.cpp
View File

@ -169,8 +169,10 @@ Mavlink::Mavlink() :
/* performance counters */ /* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")),
_mavlink_hil_enabled(false) _mavlink_hil_enabled(false),
// _params_sub(-1) // _params_sub(-1)
mission_pub(-1)
{ {
wpm = &wpm_s; wpm = &wpm_s;
fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
@ -987,6 +989,10 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
} else if (seq == 0 && wpm->size == 0) {
/* don't broadcast if no WPs */
} else { } else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
if (_verbose) warnx("ERROR: index out of bounds"); if (_verbose) warnx("ERROR: index out of bounds");
@ -1534,7 +1540,7 @@ Mavlink::task_main(int argc, char *argv[])
argc -= 2; argc -= 2;
argv += 2; argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) {
switch (ch) { switch (ch) {
case 'b': case 'b':
_baudrate = strtoul(optarg, NULL, 10); _baudrate = strtoul(optarg, NULL, 10);
@ -1558,6 +1564,7 @@ Mavlink::task_main(int argc, char *argv[])
case 'v': case 'v':
_verbose = true; _verbose = true;
break;
default: default:
usage(); usage();
@ -1778,8 +1785,9 @@ Mavlink::task_main(int argc, char *argv[])
if (_verbose) warnx("Got mission result"); if (_verbose) warnx("Got mission result");
if (mission_result.mission_reached) if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
}
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
} }