fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 00:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed compile errors, removed non-wanted MAVLink dependency in commander app

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-30 11:01:09 +01:00
parent 62a95bf8e6
commit 2577e1a749
4 changed files with 157 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+55 -81
View File
@@ -59,7 +59,6 @@
#include <errno.h>
#include <debug.h>
#include <sys/prctl.h>
#include <v1.0/common/mavlink.h>
#include <string.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -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, &current_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, &current_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, &current_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, &current_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)) {
apps/commander/state_machine_helper.c
+9 -9
View File
@@ -522,20 +522,20 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
current_status->flag_control_manual_enabled = true;
/* set behaviour based on airframe */
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, set to SAS */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
} else {
/* assuming a fixed wing, set to direct pass-through */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false;
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status->flag_control_attitude_enabled = false;
current_status->flag_control_rates_enabled = false;
}
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);