mavgen_c.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 parse a MAVLink protocol XML file and generate a C implementation
00004 
00005 Copyright Andrew Tridgell 2011
00006 Released under GNU GPL version 3 or later
00007 '''
00008 
00009 import sys, textwrap, os, time
00010 from . import mavparse, mavtemplate
00011 
00012 t = mavtemplate.MAVTemplate()
00013 
00014 def generate_version_h(directory, xml):
00015     '''generate version.h'''
00016     f = open(os.path.join(directory, "version.h"), mode='w')
00017     t.write(f,'''
00018 /** @file
00019  *      @brief MAVLink comm protocol built from ${basename}.xml
00020  *      @see http://mavlink.org
00021  */
00022 #ifndef MAVLINK_VERSION_H
00023 #define MAVLINK_VERSION_H
00024 
00025 #define MAVLINK_BUILD_DATE "${parse_time}"
00026 #define MAVLINK_WIRE_PROTOCOL_VERSION "${wire_protocol_version}"
00027 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE ${largest_payload}
00028  
00029 #endif // MAVLINK_VERSION_H
00030 ''', xml)
00031     f.close()
00032 
00033 def generate_mavlink_h(directory, xml):
00034     '''generate mavlink.h'''
00035     f = open(os.path.join(directory, "mavlink.h"), mode='w')
00036     t.write(f,'''
00037 /** @file
00038  *      @brief MAVLink comm protocol built from ${basename}.xml
00039  *      @see http://mavlink.org
00040  */
00041 #ifndef MAVLINK_H
00042 #define MAVLINK_H
00043 
00044 #ifndef MAVLINK_STX
00045 #define MAVLINK_STX ${protocol_marker}
00046 #endif
00047 
00048 #ifndef MAVLINK_ENDIAN
00049 #define MAVLINK_ENDIAN ${mavlink_endian}
00050 #endif
00051 
00052 #ifndef MAVLINK_ALIGNED_FIELDS
00053 #define MAVLINK_ALIGNED_FIELDS ${aligned_fields_define}
00054 #endif
00055 
00056 #ifndef MAVLINK_CRC_EXTRA
00057 #define MAVLINK_CRC_EXTRA ${crc_extra_define}
00058 #endif
00059 
00060 #include "version.h"
00061 #include "${basename}.h"
00062 
00063 #endif // MAVLINK_H
00064 ''', xml)
00065     f.close()
00066 
00067 def generate_main_h(directory, xml):
00068     '''generate main header per XML file'''
00069     f = open(os.path.join(directory, xml.basename + ".h"), mode='w')
00070     t.write(f, '''
00071 /** @file
00072  *      @brief MAVLink comm protocol generated from ${basename}.xml
00073  *      @see http://mavlink.org
00074  */
00075 #ifndef MAVLINK_${basename_upper}_H
00076 #define MAVLINK_${basename_upper}_H
00077 
00078 #ifndef MAVLINK_H
00079     #error Wrong include order: MAVLINK_${basename_upper}.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call.
00080 #endif
00081 
00082 #ifdef __cplusplus
00083 extern "C" {
00084 #endif
00085 
00086 // MESSAGE LENGTHS AND CRCS
00087 
00088 #ifndef MAVLINK_MESSAGE_LENGTHS
00089 #define MAVLINK_MESSAGE_LENGTHS {${message_lengths_array}}
00090 #endif
00091 
00092 #ifndef MAVLINK_MESSAGE_CRCS
00093 #define MAVLINK_MESSAGE_CRCS {${message_crcs_array}}
00094 #endif
00095 
00096 #ifndef MAVLINK_MESSAGE_INFO
00097 #define MAVLINK_MESSAGE_INFO {${message_info_array}}
00098 #endif
00099 
00100 #include "../protocol.h"
00101 
00102 #define MAVLINK_ENABLED_${basename_upper}
00103 
00104 // ENUM DEFINITIONS
00105 
00106 ${{enum:
00107 /** @brief ${description} */
00108 #ifndef HAVE_ENUM_${name}
00109 #define HAVE_ENUM_${name}
00110 typedef enum ${name}
00111 {
00112 ${{entry:       ${name}=${value}, /* ${description} |${{param:${description}| }} */
00113 }}
00114 } ${name};
00115 #endif
00116 }}
00117 
00118 ${{include_list:#include "../${base}/${base}.h"
00119 }}
00120 
00121 // MAVLINK VERSION
00122 
00123 #ifndef MAVLINK_VERSION
00124 #define MAVLINK_VERSION ${version}
00125 #endif
00126 
00127 #if (MAVLINK_VERSION == 0)
00128 #undef MAVLINK_VERSION
00129 #define MAVLINK_VERSION ${version}
00130 #endif
00131 
00132 // MESSAGE DEFINITIONS
00133 ${{message:#include "./mavlink_msg_${name_lower}.h"
00134 }}
00135 
00136 #ifdef __cplusplus
00137 }
00138 #endif // __cplusplus
00139 #endif // MAVLINK_${basename_upper}_H
00140 ''', xml)
00141 
00142     f.close()
00143              
00144 
00145 def generate_message_h(directory, m):
00146     '''generate per-message header for a XML file'''
00147     f = open(os.path.join(directory, 'mavlink_msg_%s.h' % m.name_lower), mode='w')
00148     t.write(f, '''
00149 // MESSAGE ${name} PACKING
00150 
00151 #define MAVLINK_MSG_ID_${name} ${id}
00152 
00153 typedef struct __mavlink_${name_lower}_t
00154 {
00155 ${{ordered_fields: ${type} ${name}${array_suffix}; /*< ${description}*/
00156 }}
00157 } mavlink_${name_lower}_t;
00158 
00159 #define MAVLINK_MSG_ID_${name}_LEN ${wire_length}
00160 #define MAVLINK_MSG_ID_${id}_LEN ${wire_length}
00161 
00162 #define MAVLINK_MSG_ID_${name}_CRC ${crc_extra}
00163 #define MAVLINK_MSG_ID_${id}_CRC ${crc_extra}
00164 
00165 ${{array_fields:#define MAVLINK_MSG_${msg_name}_FIELD_${name_upper}_LEN ${array_length}
00166 }}
00167 
00168 #define MAVLINK_MESSAGE_INFO_${name} { \\
00169         "${name}", \\
00170         ${num_fields}, \\
00171         { ${{ordered_fields: { "${name}", ${c_print_format}, MAVLINK_TYPE_${type_upper}, ${array_length}, ${wire_offset}, offsetof(mavlink_${name_lower}_t, ${name}) }, \\
00172         }} } \\
00173 }
00174 
00175 
00176 /**
00177  * @brief Pack a ${name_lower} message
00178  * @param system_id ID of this system
00179  * @param component_id ID of this component (e.g. 200 for IMU)
00180  * @param msg The MAVLink message to compress the data into
00181  *
00182 ${{arg_fields: * @param ${name} ${description}
00183 }}
00184  * @return length of the message in bytes (excluding serial stream start sign)
00185  */
00186 static inline uint16_t mavlink_msg_${name_lower}_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
00187                                                       ${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
00188 {
00189 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
00190         char buf[MAVLINK_MSG_ID_${name}_LEN];
00191 ${{scalar_fields:       _mav_put_${type}(buf, ${wire_offset}, ${putname});
00192 }}
00193 ${{array_fields:        _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
00194 }}
00195         memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_${name}_LEN);
00196 #else
00197         mavlink_${name_lower}_t packet;
00198 ${{scalar_fields:       packet.${name} = ${putname};
00199 }}
00200 ${{array_fields:        mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
00201 }}
00202         memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_${name}_LEN);
00203 #endif
00204 
00205         msg->msgid = MAVLINK_MSG_ID_${name};
00206 #if MAVLINK_CRC_EXTRA
00207     return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
00208 #else
00209     return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_${name}_LEN);
00210 #endif
00211 }
00212 
00213 /**
00214  * @brief Pack a ${name_lower} message on a channel
00215  * @param system_id ID of this system
00216  * @param component_id ID of this component (e.g. 200 for IMU)
00217  * @param chan The MAVLink channel this message will be sent over
00218  * @param msg The MAVLink message to compress the data into
00219 ${{arg_fields: * @param ${name} ${description}
00220 }}
00221  * @return length of the message in bytes (excluding serial stream start sign)
00222  */
00223 static inline uint16_t mavlink_msg_${name_lower}_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
00224                                                            mavlink_message_t* msg,
00225                                                            ${{arg_fields:${array_const}${type} ${array_prefix}${name},}})
00226 {
00227 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
00228         char buf[MAVLINK_MSG_ID_${name}_LEN];
00229 ${{scalar_fields:       _mav_put_${type}(buf, ${wire_offset}, ${putname});
00230 }}
00231 ${{array_fields:        _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
00232 }}
00233         memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_${name}_LEN);
00234 #else
00235         mavlink_${name_lower}_t packet;
00236 ${{scalar_fields:       packet.${name} = ${putname};
00237 }}
00238 ${{array_fields:        mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
00239 }}
00240         memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_${name}_LEN);
00241 #endif
00242 
00243         msg->msgid = MAVLINK_MSG_ID_${name};
00244 #if MAVLINK_CRC_EXTRA
00245     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
00246 #else
00247     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_${name}_LEN);
00248 #endif
00249 }
00250 
00251 /**
00252  * @brief Encode a ${name_lower} struct
00253  *
00254  * @param system_id ID of this system
00255  * @param component_id ID of this component (e.g. 200 for IMU)
00256  * @param msg The MAVLink message to compress the data into
00257  * @param ${name_lower} C-struct to read the message contents from
00258  */
00259 static inline uint16_t mavlink_msg_${name_lower}_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_${name_lower}_t* ${name_lower})
00260 {
00261         return mavlink_msg_${name_lower}_pack(system_id, component_id, msg,${{arg_fields: ${name_lower}->${name},}});
00262 }
00263 
00264 /**
00265  * @brief Encode a ${name_lower} struct on a channel
00266  *
00267  * @param system_id ID of this system
00268  * @param component_id ID of this component (e.g. 200 for IMU)
00269  * @param chan The MAVLink channel this message will be sent over
00270  * @param msg The MAVLink message to compress the data into
00271  * @param ${name_lower} C-struct to read the message contents from
00272  */
00273 static inline uint16_t mavlink_msg_${name_lower}_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_${name_lower}_t* ${name_lower})
00274 {
00275         return mavlink_msg_${name_lower}_pack_chan(system_id, component_id, chan, msg,${{arg_fields: ${name_lower}->${name},}});
00276 }
00277 
00278 /**
00279  * @brief Send a ${name_lower} message
00280  * @param chan MAVLink channel to send the message
00281  *
00282 ${{arg_fields: * @param ${name} ${description}
00283 }}
00284  */
00285 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00286 
00287 static inline void mavlink_msg_${name_lower}_send(mavlink_channel_t chan,${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
00288 {
00289 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
00290         char buf[MAVLINK_MSG_ID_${name}_LEN];
00291 ${{scalar_fields:       _mav_put_${type}(buf, ${wire_offset}, ${putname});
00292 }}
00293 ${{array_fields:        _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
00294 }}
00295 #if MAVLINK_CRC_EXTRA
00296     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
00297 #else
00298     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, MAVLINK_MSG_ID_${name}_LEN);
00299 #endif
00300 #else
00301         mavlink_${name_lower}_t packet;
00302 ${{scalar_fields:       packet.${name} = ${putname};
00303 }}
00304 ${{array_fields:        mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
00305 }}
00306 #if MAVLINK_CRC_EXTRA
00307     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)&packet, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
00308 #else
00309     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)&packet, MAVLINK_MSG_ID_${name}_LEN);
00310 #endif
00311 #endif
00312 }
00313 
00314 #if MAVLINK_MSG_ID_${name}_LEN <= MAVLINK_MAX_PAYLOAD_LEN
00315 /*
00316   This varient of _send() can be used to save stack space by re-using
00317   memory from the receive buffer.  The caller provides a
00318   mavlink_message_t which is the size of a full mavlink message. This
00319   is usually the receive buffer for the channel, and allows a reply to an
00320   incoming message with minimum stack space usage.
00321  */
00322 static inline void mavlink_msg_${name_lower}_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, ${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
00323 {
00324 #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
00325         char *buf = (char *)msgbuf;
00326 ${{scalar_fields:       _mav_put_${type}(buf, ${wire_offset}, ${putname});
00327 }}
00328 ${{array_fields:        _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
00329 }}
00330 #if MAVLINK_CRC_EXTRA
00331     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
00332 #else
00333     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, MAVLINK_MSG_ID_${name}_LEN);
00334 #endif
00335 #else
00336         mavlink_${name_lower}_t *packet = (mavlink_${name_lower}_t *)msgbuf;
00337 ${{scalar_fields:       packet->${name} = ${putname};
00338 }}
00339 ${{array_fields:        mav_array_memcpy(packet->${name}, ${name}, sizeof(${type})*${array_length});
00340 }}
00341 #if MAVLINK_CRC_EXTRA
00342     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)packet, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
00343 #else
00344     _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)packet, MAVLINK_MSG_ID_${name}_LEN);
00345 #endif
00346 #endif
00347 }
00348 #endif
00349 
00350 #endif
00351 
00352 // MESSAGE ${name} UNPACKING
00353 
00354 ${{fields:
00355 /**
00356  * @brief Get field ${name} from ${name_lower} message
00357  *
00358  * @return ${description}
00359  */
00360 static inline ${return_type} mavlink_msg_${name_lower}_get_${name}(const mavlink_message_t* msg${get_arg})
00361 {
00362         return _MAV_RETURN_${type}${array_tag}(msg, ${array_return_arg} ${wire_offset});
00363 }
00364 }}
00365 
00366 /**
00367  * @brief Decode a ${name_lower} message into a struct
00368  *
00369  * @param msg The message to decode
00370  * @param ${name_lower} C-struct to decode the message contents into
00371  */
00372 static inline void mavlink_msg_${name_lower}_decode(const mavlink_message_t* msg, mavlink_${name_lower}_t* ${name_lower})
00373 {
00374 #if MAVLINK_NEED_BYTE_SWAP
00375 ${{ordered_fields:      ${decode_left}mavlink_msg_${name_lower}_get_${name}(msg${decode_right});
00376 }}
00377 #else
00378         memcpy(${name_lower}, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_${name}_LEN);
00379 #endif
00380 }
00381 ''', m)
00382     f.close()
00383 
00384 
00385 def generate_testsuite_h(directory, xml):
00386     '''generate testsuite.h per XML file'''
00387     f = open(os.path.join(directory, "testsuite.h"), mode='w')
00388     t.write(f, '''
00389 /** @file
00390  *      @brief MAVLink comm protocol testsuite generated from ${basename}.xml
00391  *      @see http://qgroundcontrol.org/mavlink/
00392  */
00393 #ifndef ${basename_upper}_TESTSUITE_H
00394 #define ${basename_upper}_TESTSUITE_H
00395 
00396 #ifdef __cplusplus
00397 extern "C" {
00398 #endif
00399 
00400 #ifndef MAVLINK_TEST_ALL
00401 #define MAVLINK_TEST_ALL
00402 ${{include_list:static void mavlink_test_${base}(uint8_t, uint8_t, mavlink_message_t *last_msg);
00403 }}
00404 static void mavlink_test_${basename}(uint8_t, uint8_t, mavlink_message_t *last_msg);
00405 
00406 static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
00407 {
00408 ${{include_list:        mavlink_test_${base}(system_id, component_id, last_msg);
00409 }}
00410         mavlink_test_${basename}(system_id, component_id, last_msg);
00411 }
00412 #endif
00413 
00414 ${{include_list:#include "../${base}/testsuite.h"
00415 }}
00416 
00417 ${{message:
00418 static void mavlink_test_${name_lower}(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
00419 {
00420         mavlink_message_t msg;
00421         uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
00422         uint16_t i;
00423         mavlink_${name_lower}_t packet_in = {
00424                 ${{ordered_fields:${c_test_value},}}
00425     };
00426         mavlink_${name_lower}_t packet1, packet2;
00427         memset(&packet1, 0, sizeof(packet1));
00428         ${{scalar_fields:       packet1.${name} = packet_in.${name};
00429         }}
00430         ${{array_fields:        mav_array_memcpy(packet1.${name}, packet_in.${name}, sizeof(${type})*${array_length});
00431         }}
00432 
00433         memset(&packet2, 0, sizeof(packet2));
00434         mavlink_msg_${name_lower}_encode(system_id, component_id, &msg, &packet1);
00435         mavlink_msg_${name_lower}_decode(&msg, &packet2);
00436         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
00437 
00438         memset(&packet2, 0, sizeof(packet2));
00439         mavlink_msg_${name_lower}_pack(system_id, component_id, &msg ${{arg_fields:, packet1.${name} }});
00440         mavlink_msg_${name_lower}_decode(&msg, &packet2);
00441         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
00442 
00443         memset(&packet2, 0, sizeof(packet2));
00444         mavlink_msg_${name_lower}_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg ${{arg_fields:, packet1.${name} }});
00445         mavlink_msg_${name_lower}_decode(&msg, &packet2);
00446         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
00447 
00448         memset(&packet2, 0, sizeof(packet2));
00449         mavlink_msg_to_send_buffer(buffer, &msg);
00450         for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
00451                 comm_send_ch(MAVLINK_COMM_0, buffer[i]);
00452         }
00453         mavlink_msg_${name_lower}_decode(last_msg, &packet2);
00454         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
00455         
00456         memset(&packet2, 0, sizeof(packet2));
00457         mavlink_msg_${name_lower}_send(MAVLINK_COMM_1 ${{arg_fields:, packet1.${name} }});
00458         mavlink_msg_${name_lower}_decode(last_msg, &packet2);
00459         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
00460 }
00461 }}
00462 
00463 static void mavlink_test_${basename}(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
00464 {
00465 ${{message:     mavlink_test_${name_lower}(system_id, component_id, last_msg);
00466 }}
00467 }
00468 
00469 #ifdef __cplusplus
00470 }
00471 #endif // __cplusplus
00472 #endif // ${basename_upper}_TESTSUITE_H
00473 ''', xml)
00474 
00475     f.close()
00476 
00477 def copy_fixed_headers(directory, xml):
00478     '''copy the fixed protocol headers to the target directory'''
00479     import shutil
00480     hlist = [ 'protocol.h', 'mavlink_helpers.h', 'mavlink_types.h', 'checksum.h', 'mavlink_conversions.h' ]
00481     basepath = os.path.dirname(os.path.realpath(__file__))
00482     srcpath = os.path.join(basepath, 'C/include_v%s' % xml.wire_protocol_version)
00483     print("Copying fixed headers")
00484     for h in hlist:
00485         if (not ((h == 'mavlink_conversions.h') and xml.wire_protocol_version == '0.9')):
00486            src = os.path.realpath(os.path.join(srcpath, h))
00487            dest = os.path.realpath(os.path.join(directory, h))
00488            if src == dest:
00489                continue
00490            shutil.copy(src, dest)
00491 
00492 class mav_include(object):
00493     def __init__(self, base):
00494         self.base = base
00495 
00496 def generate_one(basename, xml):
00497     '''generate headers for one XML file'''
00498 
00499     directory = os.path.join(basename, xml.basename)
00500 
00501     print("Generating C implementation in directory %s" % directory)
00502     mavparse.mkdir_p(directory)
00503 
00504     if xml.little_endian:
00505         xml.mavlink_endian = "MAVLINK_LITTLE_ENDIAN"
00506     else:
00507         xml.mavlink_endian = "MAVLINK_BIG_ENDIAN"
00508 
00509     if xml.crc_extra:
00510         xml.crc_extra_define = "1"
00511     else:
00512         xml.crc_extra_define = "0"
00513 
00514     if xml.sort_fields:
00515         xml.aligned_fields_define = "1"
00516     else:
00517         xml.aligned_fields_define = "0"
00518 
00519     # work out the included headers
00520     xml.include_list = []
00521     for i in xml.include:
00522         base = i[:-4]
00523         xml.include_list.append(mav_include(base))
00524 
00525     # form message lengths array
00526     xml.message_lengths_array = ''
00527     for mlen in xml.message_lengths:
00528         xml.message_lengths_array += '%u, ' % mlen
00529     xml.message_lengths_array = xml.message_lengths_array[:-2]
00530 
00531     # and message CRCs array
00532     xml.message_crcs_array = ''
00533     for crc in xml.message_crcs:
00534         xml.message_crcs_array += '%u, ' % crc
00535     xml.message_crcs_array = xml.message_crcs_array[:-2]
00536 
00537     # form message info array
00538     xml.message_info_array = ''
00539     for name in xml.message_names:
00540         if name is not None:
00541             xml.message_info_array += 'MAVLINK_MESSAGE_INFO_%s, ' % name
00542         else:
00543             # Several C compilers don't accept {NULL} for
00544             # multi-dimensional arrays and structs
00545             # feed the compiler a "filled" empty message
00546             xml.message_info_array += '{"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, '
00547     xml.message_info_array = xml.message_info_array[:-2]
00548 
00549     # add some extra field attributes for convenience with arrays
00550     for m in xml.message:
00551         m.msg_name = m.name
00552         if xml.crc_extra:
00553             m.crc_extra_arg = ", %s" % m.crc_extra
00554         else:
00555             m.crc_extra_arg = ""
00556         for f in m.fields:
00557             if f.print_format is None:
00558                 f.c_print_format = 'NULL'
00559             else:
00560                 f.c_print_format = '"%s"' % f.print_format
00561             if f.array_length != 0:
00562                 f.array_suffix = '[%u]' % f.array_length
00563                 f.array_prefix = '*'
00564                 f.array_tag = '_array'
00565                 f.array_arg = ', %u' % f.array_length
00566                 f.array_return_arg = '%s, %u, ' % (f.name, f.array_length)
00567                 f.array_const = 'const '
00568                 f.decode_left = ''
00569                 f.decode_right = ', %s->%s' % (m.name_lower, f.name)
00570                 f.return_type = 'uint16_t'
00571                 f.get_arg = ', %s *%s' % (f.type, f.name)
00572                 if f.type == 'char':
00573                     f.c_test_value = '"%s"' % f.test_value
00574                 else:
00575                     test_strings = []
00576                     for v in f.test_value:
00577                         test_strings.append(str(v))
00578                     f.c_test_value = '{ %s }' % ', '.join(test_strings)
00579             else:
00580                 f.array_suffix = ''
00581                 f.array_prefix = ''
00582                 f.array_tag = ''
00583                 f.array_arg = ''
00584                 f.array_return_arg = ''
00585                 f.array_const = ''
00586                 f.decode_left = "%s->%s = " % (m.name_lower, f.name)
00587                 f.decode_right = ''
00588                 f.get_arg = ''
00589                 f.return_type = f.type
00590                 if f.type == 'char':
00591                     f.c_test_value = "'%s'" % f.test_value
00592                 elif f.type == 'uint64_t':
00593                     f.c_test_value = "%sULL" % f.test_value                    
00594                 elif f.type == 'int64_t':
00595                     f.c_test_value = "%sLL" % f.test_value                    
00596                 else:
00597                     f.c_test_value = f.test_value
00598 
00599     # cope with uint8_t_mavlink_version
00600     for m in xml.message:
00601         m.arg_fields = []
00602         m.array_fields = []
00603         m.scalar_fields = []
00604         for f in m.ordered_fields:
00605             if f.array_length != 0:
00606                 m.array_fields.append(f)
00607             else:
00608                 m.scalar_fields.append(f)
00609         for f in m.fields:
00610             if not f.omit_arg:
00611                 m.arg_fields.append(f)
00612                 f.putname = f.name
00613             else:
00614                 f.putname = f.const_value
00615 
00616     generate_mavlink_h(directory, xml)
00617     generate_version_h(directory, xml)
00618     generate_main_h(directory, xml)
00619     for m in xml.message:
00620         generate_message_h(directory, m)
00621     generate_testsuite_h(directory, xml)
00622 
00623 
00624 def generate(basename, xml_list):
00625     '''generate complete MAVLink C implemenation'''
00626 
00627     for xml in xml_list:
00628         generate_one(basename, xml)
00629     copy_fixed_headers(basename, xml_list[0])


mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:17