Rejecting arming if safety switch is not in safe position, added reboot command

This commit is contained in:
Lorenz Meier 2013-08-15 09:52:22 +02:00
parent 0bbc4b7012
commit cc9f1e81ad
3 changed files with 58 additions and 18 deletions

View File

@ -265,7 +265,7 @@ void usage(const char *reason)
exit(1); exit(1);
} }
void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{ {
/* result of the command */ /* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
@ -282,7 +282,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
transition_result_t arming_res = TRANSITION_NOT_CHANGED; transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
if (arming_res == TRANSITION_CHANGED) { if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
@ -291,7 +291,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
} else { } else {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_res = arming_state_transition(status, new_arming_state, armed); arming_res = arming_state_transition(status, safety, new_arming_state, armed);
if (arming_res == TRANSITION_CHANGED) { if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@ -356,6 +356,23 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
break; break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_safe(status, safety, armed)) {
if (((int)(cmd->param1)) == 1) {
/* reboot */
up_systemreset();
} else if (((int)(cmd->param1)) == 3) {
/* reboot to bootloader */
// XXX implement
result = VEHICLE_CMD_RESULT_UNSUPPORTED;
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
break; break;
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
@ -388,7 +405,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
/* check if we have new task and no other task is scheduled */ /* check if we have new task and no other task is scheduled */
if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) {
/* try to go to INIT/PREFLIGHT arming state */ /* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) {
result = VEHICLE_CMD_RESULT_ACCEPTED; result = VEHICLE_CMD_RESULT_ACCEPTED;
low_prio_task = new_low_prio_task; low_prio_task = new_low_prio_task;
@ -407,10 +424,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE;
if (((int)(cmd->param1)) == 0) { if (((int)(cmd->param1)) == 0) {
low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD;
} else if (((int)(cmd->param1)) == 1) { } else if (((int)(cmd->param1)) == 1) {
low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE;
} }
/* check if we have new task and no other task is scheduled */ /* check if we have new task and no other task is scheduled */
@ -605,6 +622,8 @@ int commander_thread_main(int argc, char *argv[])
int safety_sub = orb_subscribe(ORB_ID(safety)); int safety_sub = orb_subscribe(ORB_ID(safety));
struct safety_s safety; struct safety_s safety;
memset(&safety, 0, sizeof(safety)); memset(&safety, 0, sizeof(safety));
safety.safety_switch_available = false;
safety.safety_off = false;
/* Subscribe to manual control data */ /* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -746,7 +765,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */ /* handle it */
handle_command(&status, &control_mode, &cmd, &armed); handle_command(&status, &safety, &control_mode, &cmd, &armed);
} }
/* update safety topic */ /* update safety topic */
@ -871,7 +890,7 @@ int commander_thread_main(int argc, char *argv[])
critical_battery_voltage_actions_done = true; critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
status_changed = true; status_changed = true;
} }
@ -887,7 +906,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors // XXX check for sensors
arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
} else { } else {
// XXX: Add emergency stuff if sensors are lost // XXX: Add emergency stuff if sensors are lost
@ -1026,7 +1045,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
res = arming_state_transition(&status, new_arming_state, &armed); res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0; stick_off_counter = 0;
} else { } else {
@ -1045,7 +1064,7 @@ int commander_thread_main(int argc, char *argv[])
status.main_state == MAIN_STATE_MANUAL) { status.main_state == MAIN_STATE_MANUAL) {
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
stick_on_counter = 0; stick_on_counter = 0;
} else { } else {
@ -1168,10 +1187,10 @@ int commander_thread_main(int argc, char *argv[])
// XXX check if this is correct // XXX check if this is correct
/* arm / disarm on request */ /* arm / disarm on request */
if (sp_offboard.armed && !armed.armed) { if (sp_offboard.armed && !armed.armed) {
arming_state_transition(&status, ARMING_STATE_ARMED, &armed); arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
} else if (!sp_offboard.armed && armed.armed) { } else if (!sp_offboard.armed && armed.armed) {
arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
} }
} else { } else {
@ -1243,7 +1262,7 @@ int commander_thread_main(int argc, char *argv[])
} }
/* play arming and battery warning tunes */ /* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { if (!arm_tune_played && armed.armed) {
/* play tune when armed */ /* play tune when armed */
if (tune_arm() == OK) if (tune_arm() == OK)
arm_tune_played = true; arm_tune_played = true;
@ -1540,6 +1559,8 @@ void *commander_low_prio_loop(void *arg)
/* Set thread name */ /* Set thread name */
prctl(PR_SET_NAME, "commander_low_prio", getpid()); prctl(PR_SET_NAME, "commander_low_prio", getpid());
low_prio_task = LOW_PRIO_TASK_NONE;
while (!thread_should_exit) { while (!thread_should_exit) {
switch (low_prio_task) { switch (low_prio_task) {

View File

@ -67,7 +67,7 @@ static bool main_state_changed = true;
static bool navigation_state_changed = true; static bool navigation_state_changed = true;
transition_result_t transition_result_t
arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
{ {
transition_result_t ret = TRANSITION_DENIED; transition_result_t ret = TRANSITION_DENIED;
@ -108,8 +108,10 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi
case ARMING_STATE_ARMED: case ARMING_STATE_ARMED:
/* allow arming from STANDBY and IN-AIR-RESTORE */ /* allow arming from STANDBY and IN-AIR-RESTORE */
if (status->arming_state == ARMING_STATE_STANDBY if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */
{
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
armed->armed = true; armed->armed = true;
armed->ready_to_arm = false; armed->ready_to_arm = false;
@ -172,6 +174,19 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi
return ret; return ret;
} }
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed)
{
// System is safe if:
// 1) Not armed
// 2) Armed, but in software lockdown (HIL)
// 3) Safety switch is present AND engaged -> actuators locked
if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
return true;
} else {
return false;
}
}
bool bool
check_arming_state_changed() check_arming_state_changed()
{ {

View File

@ -47,6 +47,7 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
typedef enum { typedef enum {
@ -56,7 +57,10 @@ typedef enum {
} transition_result_t; } transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
bool check_arming_state_changed(); bool check_arming_state_changed();