Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Working version with priorities set and update time display

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #ifndef MAVLINK_TYPES_H_
shimniok 0:a6a169de725f 2 #define MAVLINK_TYPES_H_
shimniok 0:a6a169de725f 3
shimniok 0:a6a169de725f 4 #include "inttypes.h"
shimniok 0:a6a169de725f 5
shimniok 0:a6a169de725f 6 enum MAV_CLASS
shimniok 0:a6a169de725f 7 {
shimniok 0:a6a169de725f 8 MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything
shimniok 0:a6a169de725f 9 MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch
shimniok 0:a6a169de725f 10 MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu
shimniok 0:a6a169de725f 11 MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com
shimniok 0:a6a169de725f 12 MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org
shimniok 0:a6a169de725f 13 MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints
shimniok 0:a6a169de725f 14 MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands
shimniok 0:a6a169de725f 15 MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set
shimniok 0:a6a169de725f 16 MAV_CLASS_NONE = 8, ///< No valid autopilot
shimniok 0:a6a169de725f 17 MAV_CLASS_NB ///< Number of autopilot classes
shimniok 0:a6a169de725f 18 };
shimniok 0:a6a169de725f 19
shimniok 0:a6a169de725f 20 enum MAV_ACTION
shimniok 0:a6a169de725f 21 {
shimniok 0:a6a169de725f 22 MAV_ACTION_HOLD = 0,
shimniok 0:a6a169de725f 23 MAV_ACTION_MOTORS_START = 1,
shimniok 0:a6a169de725f 24 MAV_ACTION_LAUNCH = 2,
shimniok 0:a6a169de725f 25 MAV_ACTION_RETURN = 3,
shimniok 0:a6a169de725f 26 MAV_ACTION_EMCY_LAND = 4,
shimniok 0:a6a169de725f 27 MAV_ACTION_EMCY_KILL = 5,
shimniok 0:a6a169de725f 28 MAV_ACTION_CONFIRM_KILL = 6,
shimniok 0:a6a169de725f 29 MAV_ACTION_CONTINUE = 7,
shimniok 0:a6a169de725f 30 MAV_ACTION_MOTORS_STOP = 8,
shimniok 0:a6a169de725f 31 MAV_ACTION_HALT = 9,
shimniok 0:a6a169de725f 32 MAV_ACTION_SHUTDOWN = 10,
shimniok 0:a6a169de725f 33 MAV_ACTION_REBOOT = 11,
shimniok 0:a6a169de725f 34 MAV_ACTION_SET_MANUAL = 12,
shimniok 0:a6a169de725f 35 MAV_ACTION_SET_AUTO = 13,
shimniok 0:a6a169de725f 36 MAV_ACTION_STORAGE_READ = 14,
shimniok 0:a6a169de725f 37 MAV_ACTION_STORAGE_WRITE = 15,
shimniok 0:a6a169de725f 38 MAV_ACTION_CALIBRATE_RC = 16,
shimniok 0:a6a169de725f 39 MAV_ACTION_CALIBRATE_GYRO = 17,
shimniok 0:a6a169de725f 40 MAV_ACTION_CALIBRATE_MAG = 18,
shimniok 0:a6a169de725f 41 MAV_ACTION_CALIBRATE_ACC = 19,
shimniok 0:a6a169de725f 42 MAV_ACTION_CALIBRATE_PRESSURE = 20,
shimniok 0:a6a169de725f 43 MAV_ACTION_REC_START = 21,
shimniok 0:a6a169de725f 44 MAV_ACTION_REC_PAUSE = 22,
shimniok 0:a6a169de725f 45 MAV_ACTION_REC_STOP = 23,
shimniok 0:a6a169de725f 46 MAV_ACTION_TAKEOFF = 24,
shimniok 0:a6a169de725f 47 MAV_ACTION_NAVIGATE = 25,
shimniok 0:a6a169de725f 48 MAV_ACTION_LAND = 26,
shimniok 0:a6a169de725f 49 MAV_ACTION_LOITER = 27,
shimniok 0:a6a169de725f 50 MAV_ACTION_SET_ORIGIN = 28,
shimniok 0:a6a169de725f 51 MAV_ACTION_RELAY_ON = 29,
shimniok 0:a6a169de725f 52 MAV_ACTION_RELAY_OFF = 30,
shimniok 0:a6a169de725f 53 MAV_ACTION_GET_IMAGE = 31,
shimniok 0:a6a169de725f 54 MAV_ACTION_VIDEO_START = 32,
shimniok 0:a6a169de725f 55 MAV_ACTION_VIDEO_STOP = 33,
shimniok 0:a6a169de725f 56 MAV_ACTION_RESET_MAP = 34,
shimniok 0:a6a169de725f 57 MAV_ACTION_RESET_PLAN = 35,
shimniok 0:a6a169de725f 58 MAV_ACTION_DELAY_BEFORE_COMMAND = 36,
shimniok 0:a6a169de725f 59 MAV_ACTION_ASCEND_AT_RATE = 37,
shimniok 0:a6a169de725f 60 MAV_ACTION_CHANGE_MODE = 38,
shimniok 0:a6a169de725f 61 MAV_ACTION_LOITER_MAX_TURNS = 39,
shimniok 0:a6a169de725f 62 MAV_ACTION_LOITER_MAX_TIME = 40,
shimniok 0:a6a169de725f 63 MAV_ACTION_START_HILSIM = 41,
shimniok 0:a6a169de725f 64 MAV_ACTION_STOP_HILSIM = 42,
shimniok 0:a6a169de725f 65 MAV_ACTION_NB ///< Number of MAV actions
shimniok 0:a6a169de725f 66 };
shimniok 0:a6a169de725f 67
shimniok 0:a6a169de725f 68 enum MAV_MODE
shimniok 0:a6a169de725f 69 {
shimniok 0:a6a169de725f 70 MAV_MODE_UNINIT = 0, ///< System is in undefined state
shimniok 0:a6a169de725f 71 MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe
shimniok 0:a6a169de725f 72 MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control
shimniok 0:a6a169de725f 73 MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint
shimniok 0:a6a169de725f 74 MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation
shimniok 0:a6a169de725f 75 MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use
shimniok 0:a6a169de725f 76 MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use
shimniok 0:a6a169de725f 77 MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use
shimniok 0:a6a169de725f 78 MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive
shimniok 0:a6a169de725f 79 MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back
shimniok 0:a6a169de725f 80 };
shimniok 0:a6a169de725f 81
shimniok 0:a6a169de725f 82 enum MAV_STATE
shimniok 0:a6a169de725f 83 {
shimniok 0:a6a169de725f 84 MAV_STATE_UNINIT = 0,
shimniok 0:a6a169de725f 85 MAV_STATE_BOOT,
shimniok 0:a6a169de725f 86 MAV_STATE_CALIBRATING,
shimniok 0:a6a169de725f 87 MAV_STATE_STANDBY,
shimniok 0:a6a169de725f 88 MAV_STATE_ACTIVE,
shimniok 0:a6a169de725f 89 MAV_STATE_CRITICAL,
shimniok 0:a6a169de725f 90 MAV_STATE_EMERGENCY,
shimniok 0:a6a169de725f 91 MAV_STATE_HILSIM,
shimniok 0:a6a169de725f 92 MAV_STATE_POWEROFF
shimniok 0:a6a169de725f 93 };
shimniok 0:a6a169de725f 94
shimniok 0:a6a169de725f 95 enum MAV_NAV
shimniok 0:a6a169de725f 96 {
shimniok 0:a6a169de725f 97 MAV_NAV_GROUNDED = 0,
shimniok 0:a6a169de725f 98 MAV_NAV_LIFTOFF,
shimniok 0:a6a169de725f 99 MAV_NAV_HOLD,
shimniok 0:a6a169de725f 100 MAV_NAV_WAYPOINT,
shimniok 0:a6a169de725f 101 MAV_NAV_VECTOR,
shimniok 0:a6a169de725f 102 MAV_NAV_RETURNING,
shimniok 0:a6a169de725f 103 MAV_NAV_LANDING,
shimniok 0:a6a169de725f 104 MAV_NAV_LOST,
shimniok 0:a6a169de725f 105 MAV_NAV_LOITER,
shimniok 0:a6a169de725f 106 MAV_NAV_FREE_DRIFT
shimniok 0:a6a169de725f 107 };
shimniok 0:a6a169de725f 108
shimniok 0:a6a169de725f 109 enum MAV_TYPE
shimniok 0:a6a169de725f 110 {
shimniok 0:a6a169de725f 111 MAV_GENERIC = 0,
shimniok 0:a6a169de725f 112 MAV_FIXED_WING = 1,
shimniok 0:a6a169de725f 113 MAV_QUADROTOR = 2,
shimniok 0:a6a169de725f 114 MAV_COAXIAL = 3,
shimniok 0:a6a169de725f 115 MAV_HELICOPTER = 4,
shimniok 0:a6a169de725f 116 MAV_GROUND = 5,
shimniok 0:a6a169de725f 117 OCU = 6,
shimniok 0:a6a169de725f 118 MAV_AIRSHIP = 7,
shimniok 0:a6a169de725f 119 MAV_FREE_BALLOON = 8,
shimniok 0:a6a169de725f 120 MAV_ROCKET = 9,
shimniok 0:a6a169de725f 121 UGV_GROUND_ROVER = 10,
shimniok 0:a6a169de725f 122 UGV_SURFACE_SHIP = 11
shimniok 0:a6a169de725f 123 };
shimniok 0:a6a169de725f 124
shimniok 0:a6a169de725f 125 enum MAV_AUTOPILOT_TYPE
shimniok 0:a6a169de725f 126 {
shimniok 0:a6a169de725f 127 MAV_AUTOPILOT_GENERIC = 0,
shimniok 0:a6a169de725f 128 MAV_AUTOPILOT_PIXHAWK = 1,
shimniok 0:a6a169de725f 129 MAV_AUTOPILOT_SLUGS = 2,
shimniok 0:a6a169de725f 130 MAV_AUTOPILOT_ARDUPILOTMEGA = 3,
shimniok 0:a6a169de725f 131 MAV_AUTOPILOT_NONE = 4
shimniok 0:a6a169de725f 132 };
shimniok 0:a6a169de725f 133
shimniok 0:a6a169de725f 134 enum MAV_COMPONENT
shimniok 0:a6a169de725f 135 {
shimniok 0:a6a169de725f 136 MAV_COMP_ID_GPS,
shimniok 0:a6a169de725f 137 MAV_COMP_ID_WAYPOINTPLANNER,
shimniok 0:a6a169de725f 138 MAV_COMP_ID_BLOBTRACKER,
shimniok 0:a6a169de725f 139 MAV_COMP_ID_PATHPLANNER,
shimniok 0:a6a169de725f 140 MAV_COMP_ID_AIRSLAM,
shimniok 0:a6a169de725f 141 MAV_COMP_ID_MAPPER,
shimniok 0:a6a169de725f 142 MAV_COMP_ID_CAMERA,
shimniok 0:a6a169de725f 143 MAV_COMP_ID_IMU = 200,
shimniok 0:a6a169de725f 144 MAV_COMP_ID_IMU_2 = 201,
shimniok 0:a6a169de725f 145 MAV_COMP_ID_IMU_3 = 202,
shimniok 0:a6a169de725f 146 MAV_COMP_ID_UDP_BRIDGE = 240,
shimniok 0:a6a169de725f 147 MAV_COMP_ID_UART_BRIDGE = 241,
shimniok 0:a6a169de725f 148 MAV_COMP_ID_SYSTEM_CONTROL = 250
shimniok 0:a6a169de725f 149 };
shimniok 0:a6a169de725f 150
shimniok 0:a6a169de725f 151 enum MAV_FRAME
shimniok 0:a6a169de725f 152 {
shimniok 0:a6a169de725f 153 MAV_FRAME_GLOBAL = 0,
shimniok 0:a6a169de725f 154 MAV_FRAME_LOCAL = 1,
shimniok 0:a6a169de725f 155 MAV_FRAME_MISSION = 2,
shimniok 0:a6a169de725f 156 MAV_FRAME_GLOBAL_RELATIVE_ALT = 3,
shimniok 0:a6a169de725f 157 MAV_FRAME_LOCAL_ENU = 4
shimniok 0:a6a169de725f 158 };
shimniok 0:a6a169de725f 159
shimniok 0:a6a169de725f 160 enum MAVLINK_DATA_STREAM_TYPE
shimniok 0:a6a169de725f 161 {
shimniok 0:a6a169de725f 162 MAVLINK_DATA_STREAM_IMG_JPEG,
shimniok 0:a6a169de725f 163 MAVLINK_DATA_STREAM_IMG_BMP,
shimniok 0:a6a169de725f 164 MAVLINK_DATA_STREAM_IMG_RAW8U,
shimniok 0:a6a169de725f 165 MAVLINK_DATA_STREAM_IMG_RAW32U,
shimniok 0:a6a169de725f 166 MAVLINK_DATA_STREAM_IMG_PGM,
shimniok 0:a6a169de725f 167 MAVLINK_DATA_STREAM_IMG_PNG
shimniok 0:a6a169de725f 168
shimniok 0:a6a169de725f 169 };
shimniok 0:a6a169de725f 170
shimniok 0:a6a169de725f 171 #define MAVLINK_STX 0x55 ///< Packet start sign
shimniok 0:a6a169de725f 172 #define MAVLINK_STX_LEN 1 ///< Length of start sign
shimniok 0:a6a169de725f 173 #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
shimniok 0:a6a169de725f 174
shimniok 0:a6a169de725f 175 #define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
shimniok 0:a6a169de725f 176 #define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum
shimniok 0:a6a169de725f 177 #define MAVLINK_NUM_CHECKSUM_BYTES 2
shimniok 0:a6a169de725f 178 #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
shimniok 0:a6a169de725f 179 #define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN)
shimniok 0:a6a169de725f 180
shimniok 0:a6a169de725f 181 #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
shimniok 0:a6a169de725f 182 //#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN
shimniok 0:a6a169de725f 183
shimniok 0:a6a169de725f 184 typedef struct __mavlink_system {
shimniok 0:a6a169de725f 185 uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function
shimniok 0:a6a169de725f 186 uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function
shimniok 0:a6a169de725f 187 uint8_t type; ///< Unused, can be used by user to store the system's type
shimniok 0:a6a169de725f 188 uint8_t state; ///< Unused, can be used by user to store the system's state
shimniok 0:a6a169de725f 189 uint8_t mode; ///< Unused, can be used by user to store the system's mode
shimniok 0:a6a169de725f 190 uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
shimniok 0:a6a169de725f 191 } mavlink_system_t;
shimniok 0:a6a169de725f 192
shimniok 0:a6a169de725f 193 typedef struct __mavlink_message {
shimniok 0:a6a169de725f 194 uint8_t len; ///< Length of payload
shimniok 0:a6a169de725f 195 uint8_t seq; ///< Sequence of packet
shimniok 0:a6a169de725f 196 uint8_t sysid; ///< ID of message sender system/aircraft
shimniok 0:a6a169de725f 197 uint8_t compid; ///< ID of the message sender component
shimniok 0:a6a169de725f 198 uint8_t msgid; ///< ID of message in payload
shimniok 0:a6a169de725f 199 uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU
shimniok 0:a6a169de725f 200 uint8_t ck_a; ///< Checksum high byte
shimniok 0:a6a169de725f 201 uint8_t ck_b; ///< Checksum low byte
shimniok 0:a6a169de725f 202 } mavlink_message_t;
shimniok 0:a6a169de725f 203
shimniok 0:a6a169de725f 204 typedef enum {
shimniok 0:a6a169de725f 205 MAVLINK_COMM_0,
shimniok 0:a6a169de725f 206 MAVLINK_COMM_1,
shimniok 0:a6a169de725f 207 MAVLINK_COMM_2,
shimniok 0:a6a169de725f 208 MAVLINK_COMM_3
shimniok 0:a6a169de725f 209 } mavlink_channel_t;
shimniok 0:a6a169de725f 210
shimniok 0:a6a169de725f 211 /*
shimniok 0:a6a169de725f 212 * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
shimniok 0:a6a169de725f 213 * of buffers they will use. If more are used, then the result will be
shimniok 0:a6a169de725f 214 * a stack overrun
shimniok 0:a6a169de725f 215 */
shimniok 0:a6a169de725f 216 #ifndef MAVLINK_COMM_NUM_BUFFERS
shimniok 0:a6a169de725f 217 #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
shimniok 0:a6a169de725f 218 # define MAVLINK_COMM_NUM_BUFFERS 16
shimniok 0:a6a169de725f 219 #else
shimniok 0:a6a169de725f 220 # define MAVLINK_COMM_NUM_BUFFERS 4
shimniok 0:a6a169de725f 221 #endif
shimniok 0:a6a169de725f 222 #endif
shimniok 0:a6a169de725f 223
shimniok 0:a6a169de725f 224 typedef enum {
shimniok 0:a6a169de725f 225 MAVLINK_PARSE_STATE_UNINIT=0,
shimniok 0:a6a169de725f 226 MAVLINK_PARSE_STATE_IDLE,
shimniok 0:a6a169de725f 227 MAVLINK_PARSE_STATE_GOT_STX,
shimniok 0:a6a169de725f 228 MAVLINK_PARSE_STATE_GOT_SEQ,
shimniok 0:a6a169de725f 229 MAVLINK_PARSE_STATE_GOT_LENGTH,
shimniok 0:a6a169de725f 230 MAVLINK_PARSE_STATE_GOT_SYSID,
shimniok 0:a6a169de725f 231 MAVLINK_PARSE_STATE_GOT_COMPID,
shimniok 0:a6a169de725f 232 MAVLINK_PARSE_STATE_GOT_MSGID,
shimniok 0:a6a169de725f 233 MAVLINK_PARSE_STATE_GOT_PAYLOAD,
shimniok 0:a6a169de725f 234 MAVLINK_PARSE_STATE_GOT_CRC1
shimniok 0:a6a169de725f 235 } mavlink_parse_state_t; ///< The state machine for the comm parser
shimniok 0:a6a169de725f 236
shimniok 0:a6a169de725f 237 typedef struct __mavlink_status {
shimniok 0:a6a169de725f 238 uint8_t ck_a; ///< Checksum byte 1
shimniok 0:a6a169de725f 239 uint8_t ck_b; ///< Checksum byte 2
shimniok 0:a6a169de725f 240 uint8_t msg_received; ///< Number of received messages
shimniok 0:a6a169de725f 241 uint8_t buffer_overrun; ///< Number of buffer overruns
shimniok 0:a6a169de725f 242 uint8_t parse_error; ///< Number of parse errors
shimniok 0:a6a169de725f 243 mavlink_parse_state_t parse_state; ///< Parsing state machine
shimniok 0:a6a169de725f 244 uint8_t packet_idx; ///< Index in current packet
shimniok 0:a6a169de725f 245 uint8_t current_rx_seq; ///< Sequence number of last packet received
shimniok 0:a6a169de725f 246 uint8_t current_tx_seq; ///< Sequence number of last packet sent
shimniok 0:a6a169de725f 247 uint16_t packet_rx_success_count; ///< Received packets
shimniok 0:a6a169de725f 248 uint16_t packet_rx_drop_count; ///< Number of packet drops
shimniok 0:a6a169de725f 249 } mavlink_status_t;
shimniok 0:a6a169de725f 250
shimniok 0:a6a169de725f 251 #endif /* MAVLINK_TYPES_H_ */