Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
mavlink_msg_sys_status.h
00001 // MESSAGE SYS_STATUS PACKING 00002 00003 #define MAVLINK_MSG_ID_SYS_STATUS 34 00004 00005 typedef struct __mavlink_sys_status_t 00006 { 00007 uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h 00008 uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM 00009 uint8_t status; ///< System status flag, see MAV_STATUS ENUM 00010 uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 00011 uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) 00012 uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) 00013 uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) 00014 00015 } mavlink_sys_status_t; 00016 00017 00018 00019 /** 00020 * @brief Pack a sys_status message 00021 * @param system_id ID of this system 00022 * @param component_id ID of this component (e.g. 200 for IMU) 00023 * @param msg The MAVLink message to compress the data into 00024 * 00025 * @param mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h 00026 * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM 00027 * @param status System status flag, see MAV_STATUS ENUM 00028 * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 00029 * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) 00030 * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) 00031 * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) 00032 * @return length of the message in bytes (excluding serial stream start sign) 00033 */ 00034 static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) 00035 { 00036 uint16_t i = 0; 00037 msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; 00038 00039 i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h 00040 i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM 00041 i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM 00042 i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 00043 i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) 00044 i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) 00045 i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) 00046 00047 return mavlink_finalize_message(msg, system_id, component_id, i); 00048 } 00049 00050 /** 00051 * @brief Pack a sys_status message 00052 * @param system_id ID of this system 00053 * @param component_id ID of this component (e.g. 200 for IMU) 00054 * @param chan The MAVLink channel this message was sent over 00055 * @param msg The MAVLink message to compress the data into 00056 * @param mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h 00057 * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM 00058 * @param status System status flag, see MAV_STATUS ENUM 00059 * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 00060 * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) 00061 * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) 00062 * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) 00063 * @return length of the message in bytes (excluding serial stream start sign) 00064 */ 00065 static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) 00066 { 00067 uint16_t i = 0; 00068 msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; 00069 00070 i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h 00071 i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM 00072 i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM 00073 i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 00074 i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) 00075 i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) 00076 i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) 00077 00078 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); 00079 } 00080 00081 /** 00082 * @brief Encode a sys_status struct into a message 00083 * 00084 * @param system_id ID of this system 00085 * @param component_id ID of this component (e.g. 200 for IMU) 00086 * @param msg The MAVLink message to compress the data into 00087 * @param sys_status C-struct to read the message contents from 00088 */ 00089 static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) 00090 { 00091 return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->mode, sys_status->nav_mode, sys_status->status, sys_status->load, sys_status->vbat, sys_status->battery_remaining, sys_status->packet_drop); 00092 } 00093 00094 /** 00095 * @brief Send a sys_status message 00096 * @param chan MAVLink channel to send the message 00097 * 00098 * @param mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h 00099 * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM 00100 * @param status System status flag, see MAV_STATUS ENUM 00101 * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 00102 * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) 00103 * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) 00104 * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) 00105 */ 00106 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 00107 00108 static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) 00109 { 00110 mavlink_message_t msg; 00111 mavlink_msg_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop); 00112 mavlink_send_uart(chan, &msg); 00113 } 00114 00115 #endif 00116 // MESSAGE SYS_STATUS UNPACKING 00117 00118 /** 00119 * @brief Get field mode from sys_status message 00120 * 00121 * @return System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h 00122 */ 00123 static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* msg) 00124 { 00125 return (uint8_t)(msg->payload)[0]; 00126 } 00127 00128 /** 00129 * @brief Get field nav_mode from sys_status message 00130 * 00131 * @return Navigation mode, see MAV_NAV_MODE ENUM 00132 */ 00133 static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_t* msg) 00134 { 00135 return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; 00136 } 00137 00138 /** 00139 * @brief Get field status from sys_status message 00140 * 00141 * @return System status flag, see MAV_STATUS ENUM 00142 */ 00143 static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* msg) 00144 { 00145 return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; 00146 } 00147 00148 /** 00149 * @brief Get field load from sys_status message 00150 * 00151 * @return Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 00152 */ 00153 static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) 00154 { 00155 generic_16bit r; 00156 r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; 00157 r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; 00158 return (uint16_t)r.s; 00159 } 00160 00161 /** 00162 * @brief Get field vbat from sys_status message 00163 * 00164 * @return Battery voltage, in millivolts (1 = 1 millivolt) 00165 */ 00166 static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* msg) 00167 { 00168 generic_16bit r; 00169 r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; 00170 r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; 00171 return (uint16_t)r.s; 00172 } 00173 00174 /** 00175 * @brief Get field battery_remaining from sys_status message 00176 * 00177 * @return Remaining battery energy: (0%: 0, 100%: 1000) 00178 */ 00179 static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) 00180 { 00181 generic_16bit r; 00182 r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; 00183 r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; 00184 return (uint16_t)r.s; 00185 } 00186 00187 /** 00188 * @brief Get field packet_drop from sys_status message 00189 * 00190 * @return Dropped packets (packets that were corrupted on reception on the MAV) 00191 */ 00192 static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_message_t* msg) 00193 { 00194 generic_16bit r; 00195 r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; 00196 r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; 00197 return (uint16_t)r.s; 00198 } 00199 00200 /** 00201 * @brief Decode a sys_status message into a struct 00202 * 00203 * @param msg The message to decode 00204 * @param sys_status C-struct to decode the message contents into 00205 */ 00206 static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status) 00207 { 00208 sys_status->mode = mavlink_msg_sys_status_get_mode(msg); 00209 sys_status->nav_mode = mavlink_msg_sys_status_get_nav_mode(msg); 00210 sys_status->status = mavlink_msg_sys_status_get_status(msg); 00211 sys_status->load = mavlink_msg_sys_status_get_load(msg); 00212 sys_status->vbat = mavlink_msg_sys_status_get_vbat(msg); 00213 sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); 00214 sys_status->packet_drop = mavlink_msg_sys_status_get_packet_drop(msg); 00215 }
Generated on Tue Jul 12 2022 14:09:27 by 1.7.2