zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Commander update for telemetry status

sbg
Lorenz Meier 10 years ago
parent
16cb9edf19
commit
3ff8afb6ba
1 changed files with 10 additions and 10 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      src/modules/commander/commander.cpp

20
src/modules/commander/commander.cpp
Unescape View File

@ -1074,12 +1074,12 @@ int commander_thread_main(int argc, char *argv[]) @@ -1074,12 +1074,12 @@ int commander_thread_main(int argc, char *argv[])
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
/* Subscribe to telemetry status topics */
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM];
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
int telemetry_subs[ORB_MULTI_MAX_INSTANCES];
uint64_t telemetry_last_heartbeat[ORB_MULTI_MAX_INSTANCES];
uint64_t telemetry_last_dl_loss[ORB_MULTI_MAX_INSTANCES];
bool telemetry_lost[ORB_MULTI_MAX_INSTANCES];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
telemetry_subs[i] = -1;
telemetry_last_heartbeat[i] = 0;
telemetry_last_dl_loss[i] = 0;
@ -1340,10 +1340,10 @@ int commander_thread_main(int argc, char *argv[]) @@ -1340,10 +1340,10 @@ int commander_thread_main(int argc, char *argv[])
}
}
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; 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]);
if (telemetry_subs[i] < 0 && (OK == orb_exists(ORB_ID(telemetry_status), i))) {
telemetry_subs[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i);
}
orb_check(telemetry_subs[i], &updated);
@ -1352,7 +1352,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -1352,7 +1352,7 @@ int commander_thread_main(int argc, char *argv[])
struct telemetry_status_s telemetry;
memset(&telemetry, 0, sizeof(telemetry));
orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
/* perform system checks when new telemetry link connected */
if (
@ -1951,7 +1951,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -1951,7 +1951,7 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
/* handle the case where data link was gained first time or regained,

Write Preview
Loading…
Cancel
Save