forked from Archive/PX4-Autopilot
Rejecting arming if safety switch is not in safe position, added reboot command
This commit is contained in:
parent
0bbc4b7012
commit
cc9f1e81ad
|
@ -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) {
|
||||||
|
|
|
@ -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()
|
||||||
{
|
{
|
||||||
|
|
|
@ -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();
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue