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

commander: added some failsafe logic

This commit is contained in:
Julian Oes 2014-06-18 19:01:41 +02:00
parent 55e5f747de
commit e24925c743
6 changed files with 143 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -619,6 +619,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_component_id = param_find("MAV_COMP_ID");
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
/* welcome user */
warnx("starting");
@ -627,9 +628,9 @@ int commander_thread_main(int argc, char *argv[])
main_states_str[MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_ACRO] = "ACRO";
char *arming_states_str[ARMING_STATE_MAX];
@ -637,21 +638,23 @@ int commander_thread_main(int argc, char *argv[])
arming_states_str[ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[ARMING_STATE_ARMED] = "ARMED";
arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
char *nav_states_str[NAVIGATION_STATE_MAX];
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS";
nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION";
nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@ -853,6 +856,8 @@ int commander_thread_main(int argc, char *argv[])
transition_result_t arming_ret;
int32_t datalink_loss_enabled = false;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
bool main_state_changed = false;
@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[])
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
}
orb_check(sp_man_sub, &updated);
@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* data link check */
if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) {
/* handle the case where RC signal was regained */
if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
/* handle the case where data link was regained */
if (status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: data link regained");
status.data_link_lost = false;
@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
status.data_link_lost = false;
status.data_link_lost = true;
status_changed = true;
}
}
@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[])
was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status);
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.mission_finished);
// TODO handle mode changes by commands
if (main_state_changed) {
@ -1727,6 +1734,8 @@ set_control_mode()
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;

13
src/modules/commander/commander_params.c
View File

@ -39,7 +39,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
/**
* Datalink loss mode enabled.
*
* Set to 1 to enable actions triggered when the datalink is lost.
*
* @group commander
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);

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

@ -366,7 +366,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status)
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
{
navigation_state_t nav_state_old = status->nav_state;
@ -383,6 +383,16 @@ bool set_nav_state(struct vehicle_status_s *status)
if (status->rc_signal_lost && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status)
break;
case MAIN_STATE_AUTO_MISSION:
/* require data link and global position */
if ((status->data_link_lost || !status->condition_global_position_valid) && armed) {
/* go into failsafe
* - 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->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
status->failsafe = true;
} else {
if (armed) {
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost and mission is not yet finished */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for missions */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
/* require data link and local position */
if ((status->data_link_lost || !status->condition_local_position_valid) && armed) {
/* go into failsafe if datalink and RC is lost */
if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
} else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} else {
// TODO which mode should we set when disarmed?
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) {
if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
status->failsafe = true;
} else {
if (armed) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
// TODO which mode should we set when disarmed?
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
}
break;
@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status)
break;
}
if (status->failsafe) {
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
}
return status->nav_state != nav_state_old;
}

2
src/modules/commander/state_machine_helper.h
View File

@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool set_nav_state(struct vehicle_status_s *status);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
#endif /* STATE_MACHINE_HELPER_H_ */

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

@ -351,8 +351,12 @@ Navigator::task_main()
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
_navigation_mode = &_rtl; /* TODO: change this to something else */
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
default:

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

@ -101,6 +101,8 @@ typedef enum {
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */
NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */
NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */