diff options
| author | Julian Oes <julian@oes.ch> | 2014-02-23 13:42:15 +0100 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2014-02-23 13:42:15 +0100 |
| commit | 73edc020162841f250cf8ba26c6bdc40837a0e4f (patch) | |
| tree | e9bd3b73153928f8f1a03dcf4fb5cb85e5db03c7 /src/modules/mavlink/mavlink_main.cpp | |
| parent | 3ae35d8c18fa49a0a7137ab78549dc91d4ba79fa (diff) | |
| download | px4-firmware-73edc020162841f250cf8ba26c6bdc40837a0e4f.tar.gz px4-firmware-73edc020162841f250cf8ba26c6bdc40837a0e4f.tar.bz2 px4-firmware-73edc020162841f250cf8ba26c6bdc40837a0e4f.zip | |
mavlink: fix verbose mode and actually publish mission
Diffstat (limited to 'src/modules/mavlink/mavlink_main.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 14 |
1 files changed, 11 insertions, 3 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 42504dea9..15f2fd2ca 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -169,8 +169,10 @@ Mavlink::Mavlink() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), - _mavlink_hil_enabled(false) + _mavlink_hil_enabled(false), // _params_sub(-1) + + mission_pub(-1) { wpm = &wpm_s; 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); + } else if (seq == 0 && wpm->size == 0) { + + /* don't broadcast if no WPs */ + } else { mavlink_missionlib_send_gcs_string("ERROR: wp 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; argv += 2; - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1558,6 +1564,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'v': _verbose = true; + break; default: usage(); @@ -1778,8 +1785,9 @@ Mavlink::task_main(int argc, char *argv[]) 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_current((uint16_t)mission_result.index_current_mission); } |