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 /src/modules | |
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
Diffstat (limited to 'src/modules')
-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) { |