fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-25 10:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2555 from PX4/uorb_msg

Browse Source 
uORB message support
This commit is contained in:
Lorenz Meier
2015-08-12 17:31:44 +02:00
parent 5ca57a0aed 69ce66a3f9
commit 8cd6f3529c
16 changed files with 94 additions and 357 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+10 -10
View File
@@ -1081,12 +1081,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;
@@ -1347,10 +1347,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);
@@ -1359,7 +1359,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 (
@@ -1958,7 +1958,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,