Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers mavlink_msg_action.h Source File

mavlink_msg_action.h

00001 // MESSAGE ACTION PACKING
00002 
00003 #define MAVLINK_MSG_ID_ACTION 10
00004 
00005 typedef struct __mavlink_action_t 
00006 {
00007     uint8_t target; ///< The system executing the action
00008     uint8_t target_component; ///< The component executing the action
00009     uint8_t action; ///< The action id
00010 
00011 } mavlink_action_t;
00012 
00013 
00014 
00015 /**
00016  * @brief Pack a action message
00017  * @param system_id ID of this system
00018  * @param component_id ID of this component (e.g. 200 for IMU)
00019  * @param msg The MAVLink message to compress the data into
00020  *
00021  * @param target The system executing the action
00022  * @param target_component The component executing the action
00023  * @param action The action id
00024  * @return length of the message in bytes (excluding serial stream start sign)
00025  */
00026 static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action)
00027 {
00028     uint16_t i = 0;
00029     msg->msgid = MAVLINK_MSG_ID_ACTION;
00030 
00031     i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action
00032     i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action
00033     i += put_uint8_t_by_index(action, i, msg->payload); // The action id
00034 
00035     return mavlink_finalize_message(msg, system_id, component_id, i);
00036 }
00037 
00038 /**
00039  * @brief Pack a action message
00040  * @param system_id ID of this system
00041  * @param component_id ID of this component (e.g. 200 for IMU)
00042  * @param chan The MAVLink channel this message was sent over
00043  * @param msg The MAVLink message to compress the data into
00044  * @param target The system executing the action
00045  * @param target_component The component executing the action
00046  * @param action The action id
00047  * @return length of the message in bytes (excluding serial stream start sign)
00048  */
00049 static inline uint16_t mavlink_msg_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action)
00050 {
00051     uint16_t i = 0;
00052     msg->msgid = MAVLINK_MSG_ID_ACTION;
00053 
00054     i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action
00055     i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action
00056     i += put_uint8_t_by_index(action, i, msg->payload); // The action id
00057 
00058     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
00059 }
00060 
00061 /**
00062  * @brief Encode a action struct into a message
00063  *
00064  * @param system_id ID of this system
00065  * @param component_id ID of this component (e.g. 200 for IMU)
00066  * @param msg The MAVLink message to compress the data into
00067  * @param action C-struct to read the message contents from
00068  */
00069 static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_action_t* action)
00070 {
00071     return mavlink_msg_action_pack(system_id, component_id, msg, action->target, action->target_component, action->action);
00072 }
00073 
00074 /**
00075  * @brief Send a action message
00076  * @param chan MAVLink channel to send the message
00077  *
00078  * @param target The system executing the action
00079  * @param target_component The component executing the action
00080  * @param action The action id
00081  */
00082 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00083 
00084 static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action)
00085 {
00086     mavlink_message_t msg;
00087     mavlink_msg_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, target_component, action);
00088     mavlink_send_uart(chan, &msg);
00089 }
00090 
00091 #endif
00092 // MESSAGE ACTION UNPACKING
00093 
00094 /**
00095  * @brief Get field target from action message
00096  *
00097  * @return The system executing the action
00098  */
00099 static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg)
00100 {
00101     return (uint8_t)(msg->payload)[0];
00102 }
00103 
00104 /**
00105  * @brief Get field target_component from action message
00106  *
00107  * @return The component executing the action
00108  */
00109 static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_message_t* msg)
00110 {
00111     return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
00112 }
00113 
00114 /**
00115  * @brief Get field action from action message
00116  *
00117  * @return The action id
00118  */
00119 static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg)
00120 {
00121     return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
00122 }
00123 
00124 /**
00125  * @brief Decode a action message into a struct
00126  *
00127  * @param msg The message to decode
00128  * @param action C-struct to decode the message contents into
00129  */
00130 static inline void mavlink_msg_action_decode(const mavlink_message_t* msg, mavlink_action_t* action)
00131 {
00132     action->target = mavlink_msg_action_get_target(msg);
00133     action->target_component = mavlink_msg_action_get_target_component(msg);
00134     action->action = mavlink_msg_action_get_action(msg);
00135 }