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

VTOL transition failsafe RTL

Browse Source
This commit is contained in:
sander
2016-02-05 13:06:51 +01:00
committed by Andreas Antener
parent 2fa7380246
commit d5eae460c0
8 changed files with 37 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+16 -5
View File
@@ -35,11 +35,12 @@
* @file commander.cpp
* Main fail-safe handling.
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Sander Smeets <sander@droneslab.com>
*/
#include <px4_config.h>
@@ -796,6 +797,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
status_local->data_link_lost_cmd = false;
status_local->gps_failure_cmd = false;
status_local->rc_signal_lost_cmd = false;
status_local->vtol_transition_failure_cmd = false;
if ((int)cmd->param2 <= 0) {
/* reset all commanded failure modes */
@@ -820,8 +822,15 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* trigger rc loss mode */
status_local->rc_signal_lost_cmd = true;
warnx("rc loss mode commanded");
} else if ((int)cmd->param2 == 5) {
/* trigger vtol transition failure mode */
status_local->vtol_transition_failure_cmd = true;
warnx("vtol transition failure mode commanded");
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
@@ -1718,6 +1727,8 @@ int commander_thread_main(int argc, char *argv[])
if (is_vtol(&status)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
status.in_transition_mode = vtol_status.vtol_in_trans_mode;
status.vtol_transition_failure = vtol_status.vtol_transition_failsafe;
status.vtol_transition_failure_cmd = vtol_status.vtol_transition_failsafe;
}
status_changed = true;
src/modules/commander/state_machine_helper.cpp
+8 -2
View File
@@ -35,8 +35,9 @@
* @file state_machine_helper.cpp
* State machine helper functions implementations
*
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@oes.ch>
* @author Sander Smeets <sander@droneslab.com>
*/
#include <stdio.h>
@@ -636,6 +637,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
* - if we have vtol transition failure
* - depending on datalink, RC and if the mission is finished */
/* first look at the commands */
@@ -647,12 +649,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->vtol_transition_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->vtol_transition_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */