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

added offboard lost actions with additional timeout

Browse Source
This commit is contained in:
Andreas Antener
2015-11-26 17:45:20 +01:00
parent e42b5804a0
commit ced8376268
6 changed files with 138 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+24 -1
View File
@@ -1161,6 +1161,8 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_component_id = param_find("MAV_COMP_ID");
param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
param_t _param_offboard_loss_act = param_find("NAV_OBL_ACT");
param_t _param_offboard_loss_rc_act = param_find("NAV_OBL_RC_ACT");
param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
@@ -1176,6 +1178,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_geofence_action = param_find("GF_ACTION");
param_t _param_disarm_land = param_find("COM_DISARM_LAND");
param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT");
param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T");
param_t _param_fmode_1 = param_find("COM_FLTMODE1");
param_t _param_fmode_2 = param_find("COM_FLTMODE2");
@@ -1256,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = true;
status_flags.offboard_control_signal_lost = true;
status.data_link_lost = true;
status_flags.offboard_control_loss_timeout = false;
status_flags.condition_system_prearm_error_reported = false;
status_flags.condition_system_hotplug_timeout = false;
@@ -1515,6 +1519,9 @@ int commander_thread_main(int argc, char *argv[])
int32_t datalink_loss_timeout = 10;
float rc_loss_timeout = 0.5;
int32_t datalink_regain_timeout = 0;
float offboard_loss_timeout = 0.0f;
int32_t offboard_loss_act = 0;
int32_t offboard_loss_rc_act = 0;
int32_t geofence_action = 0;
@@ -1609,6 +1616,9 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_geofence_action, &geofence_action);
param_get(_param_disarm_land, &disarm_when_landed);
param_get(_param_low_bat_act, &low_bat_action);
param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
param_set(_param_offboard_loss_act, &offboard_loss_act);
param_set(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
/* Autostart id */
param_get(_param_autostart_id, &autostart_id);
@@ -1651,12 +1661,23 @@ int commander_thread_main(int argc, char *argv[])
offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
if (status_flags.offboard_control_signal_lost) {
status_flags.offboard_control_signal_lost = false;
status_flags.offboard_control_loss_timeout = false;
status_changed = true;
}
} else {
if (!status_flags.offboard_control_signal_lost) {
status_flags.offboard_control_signal_lost = true;
if (offboard_loss_timeout < FLT_EPSILON) {
/* execute loss action immediately */
status_flags.offboard_control_loss_timeout = true;
} else {
/* wait for timeout if set */
status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp +
OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f > hrt_absolute_time();
}
status_changed = true;
}
}
@@ -2669,7 +2690,9 @@ int commander_thread_main(int argc, char *argv[])
mission_result.stay_in_failsafe,
&status_flags,
land_detector.landed,
(rc_loss_enabled > 0));
(rc_loss_enabled > 0),
offboard_loss_act,
offboard_loss_rc_act);
if (status.failsafe != failsafe_old) {
status_changed = true;