dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: Only subscribe to existing telemetry status publications

This commit is contained in:
Lorenz Meier 2015-04-25 07:08:44 +02:00
parent 6e060c01a7
commit 6c9e5d1ecf
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/commander/commander.cpp
View File

@ -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) {