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

Command handling: Fix up local variable usage and status prints.

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-13 11:41:39 +02:00
parent 259014b0d5
commit ea11bc6c4b
1 changed files with 38 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+38 -29
View File
@@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
return arming_res; return arming_res;
} }
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
{ {
/* only handle commands that are meant to be handled by this system and component */ /* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
return false; return false;
} }
@@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* set HIL state */ /* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
// Transition the arming state // Transition the arming state
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
@@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* use autopilot-specific mode */ /* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status, MAIN_STATE_MANUAL); main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */ /* ALTCTL */
main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status, MAIN_STATE_POSCTL); main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */ /* AUTO */
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */ /* ACRO */
main_ret = main_state_transition(status, MAIN_STATE_ACRO); main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */ /* OFFBOARD */
main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
} }
} else { } else {
/* use base mode */ /* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */ /* AUTO */
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */ /* POSCTL */
main_ret = main_state_transition(status, MAIN_STATE_POSCTL); main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status, MAIN_STATE_MANUAL); main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
} }
} }
} }
@@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break; break;
case VEHICLE_CMD_COMPONENT_ARM_DISARM: { case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// We use an float epsilon delta to test float equality. // for logic state parameters
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
} else { } else {
bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1);
// Flick to inair restore first if this comes from an onboard system // Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
status->arming_state = ARMING_STATE_IN_AIR_RESTORE; status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
} }
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) { if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else { } else {
@@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_OVERRIDE_GOTO: { case VEHICLE_CMD_OVERRIDE_GOTO: {
// TODO listen vehicle_command topic directly from navigator (?) // TODO listen vehicle_command topic directly from navigator (?)
unsigned int mav_goto = cmd->param1;
// Increase by 0.5f and rely on the integer cast
// implicit floor(). This is the *safest* way to
// convert from floats representing small ints to actual ints.
unsigned int mav_goto = (cmd->param1 + 0.5f);
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->nav_state = NAVIGATION_STATE_AUTO_LOITER; status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); mavlink_log_critical(mavlink_fd, "Pause mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status->nav_state = NAVIGATION_STATE_AUTO_MISSION; status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); mavlink_log_critical(mavlink_fd, "Continue mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
(double)cmd->param1, (double)cmd->param1,
(double)cmd->param2, (double)cmd->param2,
(double)cmd->param3, (double)cmd->param3,
@@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
//XXX: to enable the parachute, a param needs to be set //XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (use_current) { if (use_current) {
/* use current position */ /* use current position */
if (status->condition_global_position_valid) { if (status_local->condition_global_position_valid) {
home->lat = global_pos->lat; home->lat = global_pos->lat;
home->lon = global_pos->lon; home->lon = global_pos->lon;
home->alt = global_pos->alt; home->alt = global_pos->alt;
@@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} }
/* mark home position as set */ /* mark home position as set */
status->condition_home_position_valid = true; status_local->condition_home_position_valid = true;
} }
} }
break; break;