From 2577e1a749e72814146b96c3e0473fbfb8b15a71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Dec 2012 11:01:09 +0100 Subject: Removed compile errors, removed non-wanted MAVLink dependency in commander app --- apps/commander/commander.c | 136 ++++++++++++++++++--------------------------- 1 file changed, 55 insertions(+), 81 deletions(-) (limited to 'apps/commander/commander.c') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index ab50bab16..f47e48191 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -59,7 +59,6 @@ #include #include #include -#include #include #include #include @@ -773,7 +772,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) { /* result of the command */ - uint8_t result = MAV_RESULT_UNSUPPORTED; + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* announce command handling */ tune_confirm(); @@ -783,99 +782,74 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { - case MAV_CMD_DO_SET_MODE: + case VEHICLE_CMD_DO_SET_MODE: { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } break; - case MAV_CMD_COMPONENT_ARM_DISARM: { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } } break; /* request for an autopilot reboot */ - case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { /* system may return here */ - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } } break; - case PX4_CMD_CONTROLLER_SELECTION: { - bool changed = false; - if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) { - current_vehicle_status->flag_control_rates_enabled = cmd->param1; - changed = true; - } - if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) { - current_vehicle_status->flag_control_attitude_enabled = cmd->param2; - changed = true; - } - if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) { - current_vehicle_status->flag_control_velocity_enabled = cmd->param3; - changed = true; - } - if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) { - current_vehicle_status->flag_control_position_enabled = cmd->param4; - changed = true; - } - - if (changed) { - /* publish current state machine */ - state_machine_publish(status_pub, current_vehicle_status, mavlink_fd); - } - } - // /* request to land */ -// case MAV_CMD_NAV_LAND: +// case VEHICLE_CMD_NAV_LAND: // { // //TODO: add check if landing possible // //TODO: add landing maneuver // // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = MAV_RESULT_ACCEPTED; +// result = VEHICLE_CMD_RESULT_ACCEPTED; // } } // break; // // /* request to takeoff */ -// case MAV_CMD_NAV_TAKEOFF: +// case VEHICLE_CMD_NAV_TAKEOFF: // { // //TODO: add check if takeoff possible // //TODO: add takeoff maneuver // // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = MAV_RESULT_ACCEPTED; +// result = VEHICLE_CMD_RESULT_ACCEPTED; // } // } // break; // /* preflight calibration */ - case MAV_CMD_PREFLIGHT_CALIBRATION: { + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { bool handled = false; /* gyro calibration */ @@ -890,10 +864,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] finished gyro calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING gyro calibration"); - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } @@ -910,10 +884,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] finished mag calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING mag calibration"); - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } @@ -928,7 +902,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta tune_confirm(); } else { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING altitude calibration"); - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } @@ -945,10 +919,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] finished trim calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING trim calibration"); - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } @@ -965,10 +939,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta tune_confirm(); mavlink_log_info(mavlink_fd, "[cmd] CMD finished accel calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING accel calibration"); - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } @@ -977,16 +951,16 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (!handled) { //fprintf(stderr, "[cmd] refusing unsupported calibration request\n"); mavlink_log_critical(mavlink_fd, "[cmd] CMD refusing unsup. calib. request"); - result = MAV_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } break; - case MAV_CMD_PREFLIGHT_STORAGE: { + case VEHICLE_CMD_PREFLIGHT_STORAGE: { if (current_status.flag_system_armed && - ((current_status.system_type == MAV_TYPE_QUADROTOR) || - (current_status.system_type == MAV_TYPE_HEXAROTOR) || - (current_status.system_type == MAV_TYPE_OCTOROTOR))) { + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { /* do not perform expensive memory tasks on multirotors in flight */ // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed mavlink_log_info(mavlink_fd, "[cmd] REJECTING save cmd while multicopter armed"); @@ -1003,11 +977,11 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); mavlink_log_info(mavlink_fd, "[cmd] OK loading params from"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (read_ret == 1) { mavlink_log_info(mavlink_fd, "[cmd] OK no changes in"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { if (read_ret < -1) { mavlink_log_info(mavlink_fd, "[cmd] ERR loading params from"); @@ -1016,7 +990,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] ERR no param file named"); mavlink_log_info(mavlink_fd, param_get_default_file()); } - result = MAV_RESULT_FAILED; + result = VEHICLE_CMD_RESULT_FAILED; } } else if (((int)(cmd->param1)) == 1) { @@ -1026,7 +1000,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (write_ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] OK saved param file"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { if (write_ret < -1) { @@ -1036,12 +1010,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] ERR writing params to"); mavlink_log_info(mavlink_fd, param_get_default_file()); } - result = MAV_RESULT_FAILED; + result = VEHICLE_CMD_RESULT_FAILED; } } else { mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = MAV_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } } @@ -1049,7 +1023,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = MAV_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* announce command rejection */ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -1057,11 +1031,11 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* supported command handling stop */ - if (result == MAV_RESULT_FAILED || - result == MAV_RESULT_DENIED || - result == MAV_RESULT_UNSUPPORTED) { + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED) { ioctl(buzzer, TONE_SET_ALARM, 5); - } else if (result == MAV_RESULT_ACCEPTED) { + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_confirm(); } @@ -1240,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_sys_type = param_find("VEHICLE_TYPE"); /* welcome user */ printf("[cmd] I am in command now!\n"); @@ -1411,9 +1385,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("failed setting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == MAV_TYPE_QUADROTOR || - current_status.system_type == MAV_TYPE_HEXAROTOR || - current_status.system_type == MAV_TYPE_OCTOROTOR) { + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { current_status.flag_external_manual_override_ok = false; } else { current_status.flag_external_manual_override_ok = true; @@ -1691,9 +1665,9 @@ int commander_thread_main(int argc, char *argv[]) // printf("man mode sw not finite\n"); // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == MAV_TYPE_QUADROTOR) || - // (current_status.system_type == MAV_TYPE_HEXAROTOR) || - // (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { // /* assuming a rotary wing, fall back to SAS */ // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; @@ -1710,9 +1684,9 @@ int commander_thread_main(int argc, char *argv[]) // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == MAV_TYPE_QUADROTOR) || - // (current_status.system_type == MAV_TYPE_HEXAROTOR) || - // (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { // /* assuming a rotary wing, fall back to SAS */ // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; @@ -1769,9 +1743,9 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == MAV_TYPE_QUADROTOR) || - (current_status.system_type == MAV_TYPE_HEXAROTOR) || - (current_status.system_type == MAV_TYPE_OCTOROTOR) + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) && ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { -- cgit v1.2.3