fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 18:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: Only subscribe to existing telemetry status publications

Browse Source
This commit is contained in:
Lorenz Meier
2015-04-26 14:33:56 +02:00
parent 6e060c01a7
commit 6c9e5d1ecf
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+6 -1
View File
@@ -1032,7 +1032,7 @@ int commander_thread_main(int argc, char *argv[])
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { 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_heartbeat[i] = 0;
telemetry_last_dl_loss[i] = 0; telemetry_last_dl_loss[i] = 0;
telemetry_lost[i] = true; 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++) { 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); orb_check(telemetry_subs[i], &updated);
if (updated) { if (updated) {