fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 06:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

failsafe: enable support for commands

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-14 22:38:48 +02:00
parent 9ee6ab366d
commit 0f2b66fa8b
4 changed files with 40 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+25
View File
@@ -557,6 +557,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
armed_local->force_failsafe = false;
warnx("disabling failsafe");
}
/* 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;
}
break;
src/modules/commander/state_machine_helper.cpp
+10 -1
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)) {
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -384,7 +384,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) {
src/modules/uORB/topics/vehicle_status.h
+4 -1
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;