diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-25 07:08:44 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-26 14:33:56 +0200 |
| commit | 6c9e5d1ecfa68da213d73adf279e8995f0fb28cb (patch) | |
| tree | 7ed4eb91a497c122a0ee4e8c16054679a96a1cb7 | |
| parent | 6e060c01a76401172e452e562993f79acfef9d1a (diff) | |
| download | px4-firmware-6c9e5d1ecfa68da213d73adf279e8995f0fb28cb.tar.gz px4-firmware-6c9e5d1ecfa68da213d73adf279e8995f0fb28cb.tar.bz2 px4-firmware-6c9e5d1ecfa68da213d73adf279e8995f0fb28cb.zip | |
commander: Only subscribe to existing telemetry status publications
| -rw-r--r-- | src/modules/commander/commander.cpp | 7 |
1 files changed, 6 insertions, 1 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 42338b832..57afcf19a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1032,7 +1032,7 @@ int commander_thread_main(int argc, char *argv[]) bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + telemetry_subs[i] = -1; telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; @@ -1290,6 +1290,11 @@ int commander_thread_main(int argc, char *argv[]) } for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + + if (telemetry_subs[i] < 0 && (OK == orb_exists(telemetry_status_orb_id[i], 0))) { + telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } + orb_check(telemetry_subs[i], &updated); if (updated) { |