diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 12:29:30 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 12:29:30 +0200 |
commit | c037cfe6f2daef8ff96cad965c4b040a9d8c62f9 (patch) | |
tree | ff7c0715b10bb54a1033840f6945802fb0fb404f /src/modules | |
parent | 81adc52671d920ffe184948267fcc1f9fbb027cc (diff) | |
download | px4-firmware-c037cfe6f2daef8ff96cad965c4b040a9d8c62f9.tar.gz px4-firmware-c037cfe6f2daef8ff96cad965c4b040a9d8c62f9.tar.bz2 px4-firmware-c037cfe6f2daef8ff96cad965c4b040a9d8c62f9.zip |
datalink loss (obc): add termination after loitering at airfield home
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/datalinkloss.cpp | 25 | ||||
-rw-r--r-- | src/modules/navigator/datalinkloss.h | 3 | ||||
-rw-r--r-- | src/modules/navigator/datalinkloss_params.c | 11 | ||||
-rw-r--r-- | src/modules/navigator/rcloss.cpp | 1 |
4 files changed, 39 insertions, 1 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 893d6d93a..3310984b0 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -64,6 +64,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomelat(this, "NAV_AH_LAT", false), _param_airfieldhomelon(this, "NAV_AH_LON", false), _param_airfieldhomealt(this, "NAV_AH_ALT", false), + _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { @@ -140,7 +141,8 @@ DataLinkLoss::set_dll_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -149,6 +151,15 @@ DataLinkLoss::set_dll_item() _navigator->set_can_loiter_at_sp(true); break; } + case DLL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } default: break; } @@ -185,6 +196,18 @@ DataLinkLoss::advance_dll() _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: + _dll_state = DLL_STATE_TERMINATE; + warnx("time is up, state should have been changed manually by now"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + default: break; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 96b4ce010..d0c9ad09a 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -72,12 +72,15 @@ private: control::BlockParamInt _param_airfieldhomelat; // * 1e7 control::BlockParamInt _param_airfieldhomelon; // * 1e7 control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamFloat _param_airfieldhomewaittime; control::BlockParamInt _param_numberdatalinklosses; enum DLLState { DLL_STATE_NONE = 0, DLL_STATE_FLYTOCOMMSHOLDWP = 1, DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + DLL_STATE_TERMINATE = 3, + DLL_STATE_END = 4 } _dll_state; /** diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 02f7ca4c3..db307c904 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -92,6 +92,17 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); /** + * Aifield hole wait time + * + * The amount of time in seconds the system should wait at the airfield home waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); + +/** * Number of allowed Datalink timeouts * * After more than this number of data link timeouts the aircraft returns home directly diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 427001a01..54f1813f5 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -132,6 +132,7 @@ RCLoss::set_rcl_item() pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; + break; } default: break; |