From 6c9e5d1ecfa68da213d73adf279e8995f0fb28cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 07:08:44 +0200 Subject: commander: Only subscribe to existing telemetry status publications --- src/modules/commander/commander.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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) { -- cgit v1.2.3