diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 136 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 18 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_command.h | 77 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 27 |
4 files changed, 157 insertions, 101 deletions
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 <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, ¤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)) { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index f30fd975b..fdd32ca40 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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); diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h index 3e220965d..fac571659 100644 --- a/apps/uORB/topics/vehicle_command.h +++ b/apps/uORB/topics/vehicle_command.h @@ -50,20 +50,77 @@ * @{ */ -enum PX4_CMD { - PX4_CMD_CONTROLLER_SELECTION = 1000, +/** + * Commands for commander app. + * + * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, + * but can contain additional ones. + */ +enum VEHICLE_CMD +{ + VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + VEHICLE_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + VEHICLE_CMD_ENUM_END=401, /* | */ }; +/** + * Commands for commander app. + * + * Should contain all of MAVLink's VEHICLE_CMD_RESULT values + * but can contain additional ones. + */ +enum VEHICLE_CMD_RESULT +{ + VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ + VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ + VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ + VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ + VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */ + VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ +}; + + struct vehicle_command_s { - float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */ - float param2; /**< Parameter 2, as defined by MAVLink MAV_CMD enum. */ - float param3; /**< Parameter 3, as defined by MAVLink MAV_CMD enum. */ - float param4; /**< Parameter 4, as defined by MAVLink MAV_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink MAV_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink MAV_CMD enum. */ - float param7; /**< Parameter 7, as defined by MAVLink MAV_CMD enum. */ - uint16_t command; /**< Command ID, as defined MAVLink by MAV_CMD enum. */ + float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ + float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ + float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ + float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ + float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ + uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ uint8_t target_component; /**< Component which should execute the command, 0 for all components */ uint8_t source_system; /**< System sending the command */ diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 230521f53..ccd00c52f 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -107,6 +107,31 @@ enum VEHICLE_MANUAL_SAS_MODE { }; /** + * Should match 1:1 MAVLink's MAV_TYPE ENUM + */ +enum VEHICLE_TYPE { + VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ + VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */ + VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */ + VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */ + VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + VEHICLE_TYPE_ROCKET=9, /* Rocket | */ + VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */ + VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */ + VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */ + VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */ + VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */ + VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + VEHICLE_TYPE_KITE=17, /* Kite | */ + VEHICLE_TYPE_ENUM_END=18, /* | */ +}; + +/** * state machine / state of vehicle. * * Encodes the complete system state and is set by the commander app. @@ -124,7 +149,7 @@ struct vehicle_status_s enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ - int32_t system_type; /**< system type, inspired by MAVLinks MAV_TYPE enum */ + int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ /* system flags - these represent the state predicates */ |