fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 13:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use single set of arm/disarm code

Browse Source 
Set mode and arm/disarm commands now call a single method to arm
disarm, thus removing code duplication. Also calls updated
arming_state_transition method such that the arming state logic does
not need to be duplicated outside of arming_state_transition.
This commit is contained in:
Don Gagne
2014-03-16 21:38:23 -07:00
parent ae35733381
commit dec13e7f21
1 changed files with 34 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+34 -81
View File
@@ -137,7 +137,7 @@ enum MAV_MODE_FLAG {
}; };
/* Mavlink file descriptors */ /* Mavlink file descriptors */
static int mavlink_fd; static int mavlink_fd = 0;
/* flags */ /* flags */
static bool commander_initialized = false; static bool commander_initialized = false;
@@ -217,11 +217,10 @@ void print_reject_arm(const char *msg);
void print_status(); void print_status();
int arm();
int disarm();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy);
/** /**
* Loop that runs at a lower rate and priority for calibration and parameter tasks. * Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/ */
@@ -288,12 +287,12 @@ int commander_main(int argc, char *argv[])
} }
if (!strcmp(argv[1], "arm")) { if (!strcmp(argv[1], "arm")) {
arm(); armDisarm(true, mavlink_fd, "command line");
exit(0); exit(0);
} }
if (!strcmp(argv[1], "disarm")) { if (!strcmp(argv[1], "2")) {
disarm(); armDisarm(false, mavlink_fd, "command line");
exit(0); exit(0);
} }
@@ -363,30 +362,22 @@ void print_status()
static orb_advert_t status_pub; static orb_advert_t status_pub;
int arm() transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy)
{ {
int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (arming_res == TRANSITION_CHANGED) { // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); // output appropriate error messages if the state cannot transition.
return 0; arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
} else { char buf[256];
return 1; snprintf(buf, sizeof(buf), "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} mavlink_log_info(mavlink_fd, buf);
} } else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
int disarm() }
{
int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); return arming_res;
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
} }
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
@@ -424,37 +415,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (hil_ret == OK) if (hil_ret == OK)
ret = true; ret = true;
// TODO remove debug code // Transition the arming state
//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); arming_res = armDisarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
/* set arming state */
arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
}
} else {
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_res = arming_state_transition(status, safety, new_arming_state, armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
}
} else {
arming_res = TRANSITION_NOT_CHANGED;
}
}
if (arming_res == TRANSITION_CHANGED) if (arming_res == TRANSITION_CHANGED)
ret = true; ret = true;
@@ -513,26 +475,17 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} }
case VEHICLE_CMD_COMPONENT_ARM_DISARM: { case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
transition_result_t arming_res = TRANSITION_NOT_CHANGED; // Follow exactly what the mavlink spec says for values
if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) {
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { return VEHICLE_CMD_RESULT_UNSUPPORTED;
if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { }
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED; transition_result_t arming_res = armDisarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
} else { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} } else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
} else {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} }
} }
break; break;