diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-03 10:34:52 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-03 10:34:52 +0200 |
commit | 3f8793210b47bd8e09ed2adaabc2fab966db5df6 (patch) | |
tree | abc82b24f688b1a62d09e663295da411086a8b41 /src/modules/commander | |
parent | 3d01da35d02505e751536e2cc09637797b37bed6 (diff) | |
download | px4-firmware-3f8793210b47bd8e09ed2adaabc2fab966db5df6.tar.gz px4-firmware-3f8793210b47bd8e09ed2adaabc2fab966db5df6.tar.bz2 px4-firmware-3f8793210b47bd8e09ed2adaabc2fab966db5df6.zip |
datalink check: ignore onboard computer
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 20 | ||||
-rw-r--r-- | src/modules/commander/commander_params.c | 10 |
2 files changed, 27 insertions, 3 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9885176b7..07fcb5d40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,6 +685,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); + param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); /* welcome user */ warnx("starting"); @@ -879,12 +880,14 @@ int commander_thread_main(int argc, char *argv[]) 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]; + uint8_t telemetry_sysid[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_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; + telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -971,6 +974,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; + uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid + is not validated for the datalink loss check */ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1033,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); + param_get(_param_onboard_sysid, &onboard_sysid); } orb_check(sp_man_sub, &updated); @@ -1079,6 +1085,7 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + telemetry_sysid[i] = telemetry.system_id; } } @@ -1562,10 +1569,17 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - have_link = true; - } else if (!telemetry_lost[i]) { + + /* If this is not an onboard link/onboard computer: + * set flag that we have a valid link */ + if (telemetry_sysid[i] != onboard_sysid) { + have_link = true; + } + } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout */ + * we don't have to check a timeout, + * telemetry from onboard computers is not accepted as a valid datalink + */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 30159dad9..98c0982b2 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -128,3 +128,13 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); * @max 30 */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); + +/** Onboard computer system id + * + * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check + * + * @group commander + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(COM_ONBSYSID, 42); |