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

datalink loss: add param to allow skipping of comms hold wp

This commit is contained in:
Thomas Gubler 2014-08-24 12:40:19 +02:00
parent c037cfe6f2
commit ae7c993936
3 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/navigator/datalinkloss.cpp
View File

@ -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:

1
src/modules/navigator/datalinkloss.h
View File

@ -74,6 +74,7 @@ private:
control::BlockParamFloat _param_airfieldhomealt;
control::BlockParamFloat _param_airfieldhomewaittime;
control::BlockParamInt _param_numberdatalinklosses;
control::BlockParamInt _param_skipcommshold;
enum DLLState {
DLL_STATE_NONE = 0,

14
src/modules/navigator/datalinkloss_params.c
View File

@ -107,8 +107,20 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
*
* After more than this number of data link timeouts the aircraft returns home directly
*
* @group commander
* @group DLL
* @min 0
* @max 1000
*/
PARAM_DEFINE_INT32(NAV_DLL_N, 2);
/**
* Skip comms hold wp
*
* If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
* airfield home
*
* @group DLL
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);