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

datalink loss (obc): add termination after loitering at airfield home

This commit is contained in:
Thomas Gubler 2014-08-24 12:29:30 +02:00
parent 81adc52671
commit c037cfe6f2
4 changed files with 39 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
}

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

@ -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;
/**

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

@ -91,6 +91,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
*

1
src/modules/navigator/rcloss.cpp
View File

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