diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-21 23:56:32 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-21 23:56:32 +0200 |
| commit | a9a8f1435fa798b289aa4e4af9312041abdbcf94 (patch) | |
| tree | 54e746c95f41cbecc4064031a7c426ecd96ec540 /src/modules/navigator/datalinkloss.cpp | |
| parent | 86b9e367a6cc5791f83df3223190a470798c00ff (diff) | |
| download | px4-firmware-a9a8f1435fa798b289aa4e4af9312041abdbcf94.tar.gz px4-firmware-a9a8f1435fa798b289aa4e4af9312041abdbcf94.tar.bz2 px4-firmware-a9a8f1435fa798b289aa4e4af9312041abdbcf94.zip | |
abort comm loss mode if counter above param and return home directly
Diffstat (limited to 'src/modules/navigator/datalinkloss.cpp')
| -rw-r--r-- | src/modules/navigator/datalinkloss.cpp | 14 |
1 files changed, 11 insertions, 3 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 2bd80165d..a98e21139 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,14 +57,16 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _dll_state(DLL_STATE_NONE), + _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), _param_commsholdwaittime(this, "CH_T"), _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), _param_commsholdalt(this, "CH_ALT"), _param_airfieldhomelat(this, "AH_LAT"), _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT") + _param_airfieldhomealt(this, "AH_ALT"), + _param_numberdatalinklosses(this, "DLL_N"), + _dll_state(DLL_STATE_NONE) { /* load initial params */ updateParams(); @@ -187,7 +189,13 @@ DataLinkLoss::advance_dll() { switch (_dll_state) { case DLL_STATE_NONE: - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* Check the number of data link losses. If above home fly home directly */ + updateSubscriptions(); + if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } break; case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over |