fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/obcfailsafe' into obcfailsafe

Browse Source
This commit is contained in:
Thomas Gubler 2014-08-15 14:16:01 +02:00
commit 331de9b6ad
7 changed files with 70 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/commander/commander.cpp
View File

@ -554,10 +554,35 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
warnx("forcing failsafe (termination)");
} else {
armed_local->force_failsafe = false;
warnx("disabling failsafe");
warnx("disabling failsafe (termination)");
}
/* param2 is currently used for other failsafe modes */
status_local->engine_failure_cmd = false;
status_local->data_link_lost_cmd = false;
status_local->gps_failure_cmd = false;
status_local->rc_signal_lost_cmd = false;
if ((int)cmd->param2 <= 0) {
/* reset all commanded failure modes */
warnx("revert to normal mode");
} else if ((int)cmd->param2 == 1) {
/* trigger engine failure mode */
status_local->engine_failure_cmd = true;
warnx("engine failure mode commanded");
} else if ((int)cmd->param2 == 2) {
/* trigger data link loss mode */
status_local->data_link_lost_cmd = true;
warnx("data link loss mode commanded");
} else if ((int)cmd->param2 == 3) {
/* trigger gps loss mode */
status_local->gps_failure_cmd = true;
warnx("gps loss mode commanded");
} else if ((int)cmd->param2 == 4) {
/* trigger rc loss mode */
status_local->rc_signal_lost_cmd = true;
warnx("rc loss mode commanded");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}

11
src/modules/commander/state_machine_helper.cpp
View File

@ -491,10 +491,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
if (status->engine_failure) {
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
//} else if (status->gps_failure_cmd) {
//status->nav_state = NAVIGATION_STATE_AUTO_***;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {

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

@ -58,14 +58,14 @@
DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_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_numberdatalinklosses(this, "DLL_N"),
_param_commsholdwaittime(this, "NAV_DLL_CH_T", false),
_param_commsholdlat(this, "NAV_DLL_CH_LAT", false),
_param_commsholdlon(this, "NAV_DLL_CH_LON", false),
_param_commsholdalt(this, "NAV_DLL_CH_ALT", false),
_param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false),
_param_airfieldhomelon(this, "NAV_DLL_AH_LON", false),
_param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false),
_param_numberdatalinklosses(this, "NAV_DLL_N", false),
_dll_state(DLL_STATE_NONE)
{
/* load initial params */
@ -81,7 +81,7 @@ DataLinkLoss::~DataLinkLoss()
void
DataLinkLoss::on_inactive()
{
/* reset RTL state only if setpoint moved */
/* reset DLL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_dll_state = DLL_STATE_NONE;
}
@ -90,7 +90,8 @@ DataLinkLoss::on_inactive()
void
DataLinkLoss::on_activation()
{
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
_dll_state = DLL_STATE_NONE;
advance_dll();
set_dll_item();
}
@ -131,6 +132,10 @@ DataLinkLoss::set_dll_item()
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
warnx("mission item: lat %.7f lon %.7f alt %.3f",
_mission_item.lat,
_mission_item.lon,
(double)_mission_item.altitude);
break;
}
case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
@ -159,6 +164,10 @@ DataLinkLoss::set_dll_item()
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
warnx("triplet current: lat %.7f lon %.7f alt %.3f",
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
(double)pos_sp_triplet->current.alt);
_navigator->set_position_setpoint_triplet_updated();
}
@ -166,18 +175,21 @@ DataLinkLoss::set_dll_item()
void
DataLinkLoss::advance_dll()
{
warnx("dll_state %u", _dll_state);
switch (_dll_state) {
case DLL_STATE_NONE:
/* Check the number of data link losses. If above home fly home directly */
updateSubscriptions();
if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) {
warnx("too many data link losses, fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
warnx("fly to comms hold");
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
}
break;
case DLL_STATE_FLYTOCOMMSHOLDWP:
//XXX check here if time is over are over
warnx("fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();

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

@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120);
PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
/**
* Comms hold Lon
@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810);
PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810);
/**
* Airfield home Lon

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

@ -65,9 +65,8 @@
/**
* Number of navigation modes that need on_active/on_inactive calls
* Currently: mission, loiter, and rtl
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 4
#define NAVIGATOR_MODE_ARRAY_SIZE 6
class Navigator : public control::SuperBlock
{

4
src/modules/navigator/navigator_main.cpp
View File

@ -141,6 +141,8 @@ Navigator::Navigator() :
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_offboard;
_navigation_mode_array[4] = &_dataLinkLoss;
_navigation_mode_array[5] = &_engineFailure;
updateParams();
}
@ -384,7 +386,7 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_RTL:
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
if (_param_datalinkloss_obc.get() != 0) {

5
src/modules/uORB/topics/vehicle_status.h
View File

@ -200,13 +200,16 @@ struct vehicle_status_s {
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */
bool rc_input_blocked; /**< set if RC input should be ignored */
bool data_link_lost; /**< datalink to GCS lost */
bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */
uint8_t data_link_lost_counter; /**< counts unique data link lost events */
bool engine_failure; /** Set to true if an engine failure is detected */
bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */
bool gps_failure; /** Set to true if a gps failure is detected */
bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;