zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: added some failsafe logic

sbg
Julian Oes 11 years ago
parent
55e5f747de
commit
e24925c743
6 changed files with 142 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 39
      src/modules/commander/commander.cpp
  2. 13
      src/modules/commander/commander_params.c
  3. 132
      src/modules/commander/state_machine_helper.cpp
  4. 2
      src/modules/commander/state_machine_helper.h
  5. 4
      src/modules/navigator/navigator_main.cpp
  6. 2
      src/modules/uORB/topics/vehicle_status.h

39
src/modules/commander/commander.cpp
Unescape 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_component_id = param_find("MAV_COMP_ID");
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
/* welcome user */ /* welcome user */
warnx("starting"); 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_MANUAL] = "MANUAL";
main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_ACRO] = "ACRO"; main_states_str[MAIN_STATE_ACRO] = "ACRO";
char *arming_states_str[ARMING_STATE_MAX]; 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_STANDBY] = "STANDBY";
arming_states_str[ARMING_STATE_ARMED] = "ARMED"; arming_states_str[ARMING_STATE_ARMED] = "ARMED";
arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; 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_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]; char *nav_states_str[NAVIGATION_STATE_MAX];
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; 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_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; 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 for slow low prio thread */
pthread_t commander_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; transition_result_t arming_ret;
int32_t datalink_loss_enabled = false;
/* check which state machines for changes, clear "changed" flag */ /* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false; bool arming_state_changed = false;
bool main_state_changed = false; bool main_state_changed = false;
@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[])
/* navigation parameters */ /* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled); param_get(_param_enable_parachute, &parachute_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
} }
orb_check(sp_man_sub, &updated); orb_check(sp_man_sub, &updated);
@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[])
} }
/* data link check */ /* data link check */
if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
/* handle the case where RC signal was regained */ /* handle the case where data link was regained */
if (status.data_link_lost) { if (status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: data link regained"); mavlink_log_critical(mavlink_fd, "#audio: data link regained");
status.data_link_lost = false; status.data_link_lost = false;
@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[])
} else { } else {
if (!status.data_link_lost) { if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: 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; status_changed = true;
} }
} }
@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[])
was_armed = armed.armed; was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */ /* 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 // TODO handle mode changes by commands
if (main_state_changed) { if (main_state_changed) {
@ -1727,6 +1734,8 @@ set_control_mode()
case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL: 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_manual_enabled = false;
control_mode.flag_control_auto_enabled = true; control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true; control_mode.flag_control_rates_enabled = true;

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

@ -39,7 +39,7 @@
* *
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.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> #include <nuttx/config.h>
@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
* @group Battery Calibration * @group Battery Calibration
*/ */
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); 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);

132
src/modules/commander/state_machine_helper.cpp
Unescape 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 * 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; 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) { if (status->rc_signal_lost && armed) {
status->failsafe = true; 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 { } else {
switch (status->main_state) { switch (status->main_state) {
case MAIN_STATE_ACRO: case MAIN_STATE_ACRO:
@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status)
break; break;
case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_MISSION:
/* require data link and global position */ /* go into failsafe
if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { * - 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; status->failsafe = true;
} else { if (status->condition_global_position_valid && status->condition_home_position_valid) {
if (armed) { status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } 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 { } else {
// TODO which mode should we set when disarmed? status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} }
/* 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; break;
case MAIN_STATE_AUTO_LOITER: case MAIN_STATE_AUTO_LOITER:
/* require data link and local position */ /* go into failsafe if datalink and RC is lost */
if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { 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; 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 { } else {
// TODO which mode should we set when disarmed? /* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER; status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} }
break; break;
case MAIN_STATE_AUTO_RTL: case MAIN_STATE_AUTO_RTL:
/* require global position and home */ /* 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; status->failsafe = true;
} else { if (status->condition_local_position_valid) {
if (armed) { status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else { } else {
// TODO which mode should we set when disarmed? status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} }
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} }
break; break;
@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status)
break; 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; return status->nav_state != nav_state_old;
} }

2
src/modules/commander/state_machine_helper.h
Unescape 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); 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_ */ #endif /* STATE_MACHINE_HELPER_H_ */

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

@ -351,8 +351,12 @@ Navigator::task_main()
_navigation_mode = &_loiter; _navigation_mode = &_loiter;
break; break;
case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
_navigation_mode = &_rtl; _navigation_mode = &_rtl;
break; 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_LAND:
case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_TERMINATION:
default: default:

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

@ -101,6 +101,8 @@ typedef enum {
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL 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_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */

Write Preview
Loading…
Cancel
Save