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_object_detection_event.h Source File

mavlink_msg_object_detection_event.h

00001 // MESSAGE OBJECT_DETECTION_EVENT PACKING
00002 
00003 #define MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT 140
00004 
00005 typedef struct __mavlink_object_detection_event_t 
00006 {
00007     uint32_t time; ///< Timestamp in milliseconds since system boot
00008     uint16_t object_id; ///< Object ID
00009     uint8_t type; ///< Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
00010     char name[20]; ///< Name of the object as defined by the detector
00011     uint8_t quality; ///< Detection quality / confidence. 0: bad, 255: maximum confidence
00012     float bearing; ///< Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
00013     float distance; ///< Ground distance in meters
00014 
00015 } mavlink_object_detection_event_t;
00016 
00017 #define MAVLINK_MSG_OBJECT_DETECTION_EVENT_FIELD_NAME_LEN 20
00018 
00019 
00020 /**
00021  * @brief Pack a object_detection_event message
00022  * @param system_id ID of this system
00023  * @param component_id ID of this component (e.g. 200 for IMU)
00024  * @param msg The MAVLink message to compress the data into
00025  *
00026  * @param time Timestamp in milliseconds since system boot
00027  * @param object_id Object ID
00028  * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
00029  * @param name Name of the object as defined by the detector
00030  * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence
00031  * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
00032  * @param distance Ground distance in meters
00033  * @return length of the message in bytes (excluding serial stream start sign)
00034  */
00035 static inline uint16_t mavlink_msg_object_detection_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance)
00036 {
00037     uint16_t i = 0;
00038     msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT;
00039 
00040     i += put_uint32_t_by_index(time, i, msg->payload); // Timestamp in milliseconds since system boot
00041     i += put_uint16_t_by_index(object_id, i, msg->payload); // Object ID
00042     i += put_uint8_t_by_index(type, i, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
00043     i += put_array_by_index((const int8_t*)name, sizeof(char)*20, i, msg->payload); // Name of the object as defined by the detector
00044     i += put_uint8_t_by_index(quality, i, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence
00045     i += put_float_by_index(bearing, i, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
00046     i += put_float_by_index(distance, i, msg->payload); // Ground distance in meters
00047 
00048     return mavlink_finalize_message(msg, system_id, component_id, i);
00049 }
00050 
00051 /**
00052  * @brief Pack a object_detection_event message
00053  * @param system_id ID of this system
00054  * @param component_id ID of this component (e.g. 200 for IMU)
00055  * @param chan The MAVLink channel this message was sent over
00056  * @param msg The MAVLink message to compress the data into
00057  * @param time Timestamp in milliseconds since system boot
00058  * @param object_id Object ID
00059  * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
00060  * @param name Name of the object as defined by the detector
00061  * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence
00062  * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
00063  * @param distance Ground distance in meters
00064  * @return length of the message in bytes (excluding serial stream start sign)
00065  */
00066 static inline uint16_t mavlink_msg_object_detection_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance)
00067 {
00068     uint16_t i = 0;
00069     msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT;
00070 
00071     i += put_uint32_t_by_index(time, i, msg->payload); // Timestamp in milliseconds since system boot
00072     i += put_uint16_t_by_index(object_id, i, msg->payload); // Object ID
00073     i += put_uint8_t_by_index(type, i, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
00074     i += put_array_by_index((const int8_t*)name, sizeof(char)*20, i, msg->payload); // Name of the object as defined by the detector
00075     i += put_uint8_t_by_index(quality, i, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence
00076     i += put_float_by_index(bearing, i, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
00077     i += put_float_by_index(distance, i, msg->payload); // Ground distance in meters
00078 
00079     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
00080 }
00081 
00082 /**
00083  * @brief Encode a object_detection_event struct into a message
00084  *
00085  * @param system_id ID of this system
00086  * @param component_id ID of this component (e.g. 200 for IMU)
00087  * @param msg The MAVLink message to compress the data into
00088  * @param object_detection_event C-struct to read the message contents from
00089  */
00090 static inline uint16_t mavlink_msg_object_detection_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_object_detection_event_t* object_detection_event)
00091 {
00092     return mavlink_msg_object_detection_event_pack(system_id, component_id, msg, object_detection_event->time, object_detection_event->object_id, object_detection_event->type, object_detection_event->name, object_detection_event->quality, object_detection_event->bearing, object_detection_event->distance);
00093 }
00094 
00095 /**
00096  * @brief Send a object_detection_event message
00097  * @param chan MAVLink channel to send the message
00098  *
00099  * @param time Timestamp in milliseconds since system boot
00100  * @param object_id Object ID
00101  * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
00102  * @param name Name of the object as defined by the detector
00103  * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence
00104  * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
00105  * @param distance Ground distance in meters
00106  */
00107 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00108 
00109 static inline void mavlink_msg_object_detection_event_send(mavlink_channel_t chan, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance)
00110 {
00111     mavlink_message_t msg;
00112     mavlink_msg_object_detection_event_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time, object_id, type, name, quality, bearing, distance);
00113     mavlink_send_uart(chan, &msg);
00114 }
00115 
00116 #endif
00117 // MESSAGE OBJECT_DETECTION_EVENT UNPACKING
00118 
00119 /**
00120  * @brief Get field time from object_detection_event message
00121  *
00122  * @return Timestamp in milliseconds since system boot
00123  */
00124 static inline uint32_t mavlink_msg_object_detection_event_get_time(const mavlink_message_t* msg)
00125 {
00126     generic_32bit r;
00127     r.b[3] = (msg->payload)[0];
00128     r.b[2] = (msg->payload)[1];
00129     r.b[1] = (msg->payload)[2];
00130     r.b[0] = (msg->payload)[3];
00131     return (uint32_t)r.i;
00132 }
00133 
00134 /**
00135  * @brief Get field object_id from object_detection_event message
00136  *
00137  * @return Object ID
00138  */
00139 static inline uint16_t mavlink_msg_object_detection_event_get_object_id(const mavlink_message_t* msg)
00140 {
00141     generic_16bit r;
00142     r.b[1] = (msg->payload+sizeof(uint32_t))[0];
00143     r.b[0] = (msg->payload+sizeof(uint32_t))[1];
00144     return (uint16_t)r.s;
00145 }
00146 
00147 /**
00148  * @brief Get field type from object_detection_event message
00149  *
00150  * @return Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
00151  */
00152 static inline uint8_t mavlink_msg_object_detection_event_get_type(const mavlink_message_t* msg)
00153 {
00154     return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint16_t))[0];
00155 }
00156 
00157 /**
00158  * @brief Get field name from object_detection_event message
00159  *
00160  * @return Name of the object as defined by the detector
00161  */
00162 static inline uint16_t mavlink_msg_object_detection_event_get_name(const mavlink_message_t* msg, char* r_data)
00163 {
00164 
00165     memcpy(r_data, msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t), sizeof(char)*20);
00166     return sizeof(char)*20;
00167 }
00168 
00169 /**
00170  * @brief Get field quality from object_detection_event message
00171  *
00172  * @return Detection quality / confidence. 0: bad, 255: maximum confidence
00173  */
00174 static inline uint8_t mavlink_msg_object_detection_event_get_quality(const mavlink_message_t* msg)
00175 {
00176     return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20)[0];
00177 }
00178 
00179 /**
00180  * @brief Get field bearing from object_detection_event message
00181  *
00182  * @return Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
00183  */
00184 static inline float mavlink_msg_object_detection_event_get_bearing(const mavlink_message_t* msg)
00185 {
00186     generic_32bit r;
00187     r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[0];
00188     r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[1];
00189     r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[2];
00190     r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[3];
00191     return (float)r.f;
00192 }
00193 
00194 /**
00195  * @brief Get field distance from object_detection_event message
00196  *
00197  * @return Ground distance in meters
00198  */
00199 static inline float mavlink_msg_object_detection_event_get_distance(const mavlink_message_t* msg)
00200 {
00201     generic_32bit r;
00202     r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[0];
00203     r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[1];
00204     r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[2];
00205     r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[3];
00206     return (float)r.f;
00207 }
00208 
00209 /**
00210  * @brief Decode a object_detection_event message into a struct
00211  *
00212  * @param msg The message to decode
00213  * @param object_detection_event C-struct to decode the message contents into
00214  */
00215 static inline void mavlink_msg_object_detection_event_decode(const mavlink_message_t* msg, mavlink_object_detection_event_t* object_detection_event)
00216 {
00217     object_detection_event->time = mavlink_msg_object_detection_event_get_time(msg);
00218     object_detection_event->object_id = mavlink_msg_object_detection_event_get_object_id(msg);
00219     object_detection_event->type = mavlink_msg_object_detection_event_get_type(msg);
00220     mavlink_msg_object_detection_event_get_name(msg, object_detection_event->name);
00221     object_detection_event->quality = mavlink_msg_object_detection_event_get_quality(msg);
00222     object_detection_event->bearing = mavlink_msg_object_detection_event_get_bearing(msg);
00223     object_detection_event->distance = mavlink_msg_object_detection_event_get_distance(msg);
00224 }