dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

dl loss: correct timeout, add hysteresis also for regain

This commit is contained in:
Thomas Gubler 2014-07-24 08:57:58 +02:00
parent f10d1277a5
commit a35814d15b
2 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/commander/commander.cpp
View File

@ -650,6 +650,7 @@ int commander_thread_main(int argc, char *argv[])
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_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
/* welcome user */
warnx("starting");
@ -841,11 +842,13 @@ int commander_thread_main(int argc, char *argv[])
/* 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];
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;
}
@ -930,6 +933,7 @@ 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;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
@ -990,6 +994,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_enable_parachute, &parachute_enabled);
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);
}
orb_check(sp_man_sub, &updated);
@ -1030,7 +1035,7 @@ int commander_thread_main(int argc, char *argv[])
if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) {
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
(void)rc_calibration_check(mavlink_fd);
}
@ -1459,15 +1464,21 @@ 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++) {
if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) {
/* handle the case where data link was regained */
if (telemetry_lost[i]) {//XXX also add hysteresis here
if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
/* handle the case where data link was regained,
* accept datalink as healthy only after datalink_regain_timeout seconds
* */
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
telemetry_lost[i] = false;
have_link = true;
}
have_link = true;
} else {
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
telemetry_lost[i] = true;

18
src/modules/commander/commander_params.c
View File

@ -106,13 +106,25 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
*/
PARAM_DEFINE_INT32(DL_LOSS_EN, 0);
/** Datalink timeout threshold
/** Datalink loss time threshold
*
* After this amount of seconds the data link lost mode triggers
* After this amount of seconds without datalink the data link lost mode triggers
*
* @group commander
* @unit second
* @min 0
* @max 1000
* @max 30
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
/** Datalink regain time threshold
*
* After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
* flag is set back to false
*
* @group commander
* @unit second
* @min 0
* @max 30
*/
PARAM_DEFINE_INT32(COM_DL_REG_T, 0);