diff options
Diffstat (limited to 'src/modules/commander/commander.cpp')
| -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) { |