diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 12:40:19 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-24 12:40:19 +0200 |
| commit | ae7c99393606373e3a549946481fe07de6fb4c84 (patch) | |
| tree | 984a18a6329a8818c01f73630e75eeef0c19b195 /src/modules/navigator/datalinkloss.cpp | |
| parent | c037cfe6f2daef8ff96cad965c4b040a9d8c62f9 (diff) | |
| download | px4-firmware-ae7c99393606373e3a549946481fe07de6fb4c84.tar.gz px4-firmware-ae7c99393606373e3a549946481fe07de6fb4c84.tar.bz2 px4-firmware-ae7c99393606373e3a549946481fe07de6fb4c84.zip | |
datalink loss: add param to allow skipping of comms hold wp
Diffstat (limited to 'src/modules/navigator/datalinkloss.cpp')
| -rw-r--r-- | src/modules/navigator/datalinkloss.cpp | 24 |
1 files changed, 17 insertions, 7 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 3310984b0..4e3d25840 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -66,6 +66,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomealt(this, "NAV_AH_ALT", false), _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), + _param_skipcommshold(this, "CHSK"), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -179,14 +180,23 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + if (!_param_skipcommshold.get()) { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } } else { - warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: |