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

data link loss timeout as param

Browse Source
This commit is contained in:
Thomas Gubler
2014-07-20 17:53:04 +02:00
parent 075a99e6e0
commit dcf114aa65
2 changed files with 17 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+5 -3
View File
@@ -127,7 +127,6 @@ extern struct system_load_s system_load;
#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000 #define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
#define OFFBOARD_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000 #define DIFFPRESS_TIMEOUT 2000000
@@ -650,6 +649,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
/* welcome user */ /* welcome user */
warnx("starting"); warnx("starting");
@@ -924,6 +924,7 @@ int commander_thread_main(int argc, char *argv[])
transition_result_t arming_ret; transition_result_t arming_ret;
int32_t datalink_loss_enabled = false; int32_t datalink_loss_enabled = false;
int32_t datalink_loss_timeout = 10;
/* check which state machines for changes, clear "changed" flag */ /* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false; bool arming_state_changed = false;
@@ -983,6 +984,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled); param_get(_param_enable_parachute, &parachute_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
} }
orb_check(sp_man_sub, &updated); orb_check(sp_man_sub, &updated);
@@ -1023,7 +1025,7 @@ int commander_thread_main(int argc, char *argv[])
if (mavlink_fd && if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 && telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 && telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) {
(void)rc_calibration_check(mavlink_fd); (void)rc_calibration_check(mavlink_fd);
} }
@@ -1448,7 +1450,7 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */ /* data links check */
bool have_link = false; bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) {
/* handle the case where data link was regained */ /* handle the case where data link was regained */
if (telemetry_lost[i]) { if (telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i); mavlink_log_critical(mavlink_fd, "data link %i regained", i);
src/modules/commander/commander_params.c
+12
View File
@@ -95,3 +95,15 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
* @max 1 * @max 1
*/ */
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
/**
* Datalink timeout threshold
*
* After this amount of seconds the data link lost mode triggers
*
* @group commander
* @unit second
* @min 0
* @max 1000
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);