From 8a365179eafdf3aea98e60ab9f5882b200d4c759 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 4 Aug 2012 15:12:36 -0700 Subject: Fresh import of the PX4 firmware sources. --- .../share/pyshared/pymavlink/generator/mavgen_c.py | 581 +++++++++++++++++++++ 1 file changed, 581 insertions(+) create mode 100644 mavlink/share/pyshared/pymavlink/generator/mavgen_c.py (limited to 'mavlink/share/pyshared/pymavlink/generator/mavgen_c.py') diff --git a/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py b/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py new file mode 100644 index 000000000..255919f0d --- /dev/null +++ b/mavlink/share/pyshared/pymavlink/generator/mavgen_c.py @@ -0,0 +1,581 @@ +#!/usr/bin/env python +''' +parse a MAVLink protocol XML file and generate a C implementation + +Copyright Andrew Tridgell 2011 +Released under GNU GPL version 3 or later +''' + +import sys, textwrap, os, time +import mavparse, mavtemplate + +t = mavtemplate.MAVTemplate() + +def generate_version_h(directory, xml): + '''generate version.h''' + f = open(os.path.join(directory, "version.h"), mode='w') + t.write(f,''' +/** @file + * @brief MAVLink comm protocol built from ${basename}.xml + * @see http://pixhawk.ethz.ch/software/mavlink + */ +#ifndef MAVLINK_VERSION_H +#define MAVLINK_VERSION_H + +#define MAVLINK_BUILD_DATE "${parse_time}" +#define MAVLINK_WIRE_PROTOCOL_VERSION "${wire_protocol_version}" +#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE ${largest_payload} + +#endif // MAVLINK_VERSION_H +''', xml) + f.close() + +def generate_mavlink_h(directory, xml): + '''generate mavlink.h''' + f = open(os.path.join(directory, "mavlink.h"), mode='w') + t.write(f,''' +/** @file + * @brief MAVLink comm protocol built from ${basename}.xml + * @see http://pixhawk.ethz.ch/software/mavlink + */ +#ifndef MAVLINK_H +#define MAVLINK_H + +#ifndef MAVLINK_STX +#define MAVLINK_STX ${protocol_marker} +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN ${mavlink_endian} +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS ${aligned_fields_define} +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA ${crc_extra_define} +#endif + +#include "version.h" +#include "${basename}.h" + +#endif // MAVLINK_H +''', xml) + f.close() + +def generate_main_h(directory, xml): + '''generate main header per XML file''' + f = open(os.path.join(directory, xml.basename + ".h"), mode='w') + t.write(f, ''' +/** @file + * @brief MAVLink comm protocol generated from ${basename}.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef ${basename_upper}_H +#define ${basename_upper}_H + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {${message_lengths_array}} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {${message_crcs_array}} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {${message_info_array}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_${basename_upper} + +${{include_list:#include "../${base}/${base}.h" +}} + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION ${version} +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION ${version} +#endif + +// ENUM DEFINITIONS + +${{enum: +/** @brief ${description} */ +#ifndef HAVE_ENUM_${name} +#define HAVE_ENUM_${name} +enum ${name} +{ +${{entry: ${name}=${value}, /* ${description} |${{param:${description}| }} */ +}} +}; +#endif +}} + +// MESSAGE DEFINITIONS +${{message:#include "./mavlink_msg_${name_lower}.h" +}} + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // ${basename_upper}_H +''', xml) + + f.close() + + +def generate_message_h(directory, m): + '''generate per-message header for a XML file''' + f = open(os.path.join(directory, 'mavlink_msg_%s.h' % m.name_lower), mode='w') + t.write(f, ''' +// MESSAGE ${name} PACKING + +#define MAVLINK_MSG_ID_${name} ${id} + +typedef struct __mavlink_${name_lower}_t +{ +${{ordered_fields: ${type} ${name}${array_suffix}; ///< ${description} +}} +} mavlink_${name_lower}_t; + +#define MAVLINK_MSG_ID_${name}_LEN ${wire_length} +#define MAVLINK_MSG_ID_${id}_LEN ${wire_length} + +${{array_fields:#define MAVLINK_MSG_${msg_name}_FIELD_${name_upper}_LEN ${array_length} +}} + +#define MAVLINK_MESSAGE_INFO_${name} { \\ + "${name}", \\ + ${num_fields}, \\ + { ${{ordered_fields: { "${name}", ${c_print_format}, MAVLINK_TYPE_${type_upper}, ${array_length}, ${wire_offset}, offsetof(mavlink_${name_lower}_t, ${name}) }, \\ + }} } \\ +} + + +/** + * @brief Pack a ${name_lower} message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * +${{arg_fields: * @param ${name} ${description} +}} + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_${name_lower}_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + ${{arg_fields: ${array_const}${type} ${array_prefix}${name},}}) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[${wire_length}]; +${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname}); +}} +${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length}); +}} + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length}); +#else + mavlink_${name_lower}_t packet; +${{scalar_fields: packet.${name} = ${putname}; +}} +${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length}); +}} + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length}); +#endif + + msg->msgid = MAVLINK_MSG_ID_${name}; + return mavlink_finalize_message(msg, system_id, component_id, ${wire_length}${crc_extra_arg}); +} + +/** + * @brief Pack a ${name_lower} message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into +${{arg_fields: * @param ${name} ${description} +}} + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_${name_lower}_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + ${{arg_fields:${array_const}${type} ${array_prefix}${name},}}) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[${wire_length}]; +${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname}); +}} +${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length}); +}} + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length}); +#else + mavlink_${name_lower}_t packet; +${{scalar_fields: packet.${name} = ${putname}; +}} +${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length}); +}} + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length}); +#endif + + msg->msgid = MAVLINK_MSG_ID_${name}; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, ${wire_length}${crc_extra_arg}); +} + +/** + * @brief Encode a ${name_lower} struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ${name_lower} C-struct to read the message contents from + */ +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}) +{ + return mavlink_msg_${name_lower}_pack(system_id, component_id, msg,${{arg_fields: ${name_lower}->${name},}}); +} + +/** + * @brief Send a ${name_lower} message + * @param chan MAVLink channel to send the message + * +${{arg_fields: * @param ${name} ${description} +}} + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_${name_lower}_send(mavlink_channel_t chan,${{arg_fields: ${array_const}${type} ${array_prefix}${name},}}) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[${wire_length}]; +${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname}); +}} +${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length}); +}} + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, ${wire_length}${crc_extra_arg}); +#else + mavlink_${name_lower}_t packet; +${{scalar_fields: packet.${name} = ${putname}; +}} +${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length}); +}} + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)&packet, ${wire_length}${crc_extra_arg}); +#endif +} + +#endif + +// MESSAGE ${name} UNPACKING + +${{fields: +/** + * @brief Get field ${name} from ${name_lower} message + * + * @return ${description} + */ +static inline ${return_type} mavlink_msg_${name_lower}_get_${name}(const mavlink_message_t* msg${get_arg}) +{ + return _MAV_RETURN_${type}${array_tag}(msg, ${array_return_arg} ${wire_offset}); +} +}} + +/** + * @brief Decode a ${name_lower} message into a struct + * + * @param msg The message to decode + * @param ${name_lower} C-struct to decode the message contents into + */ +static inline void mavlink_msg_${name_lower}_decode(const mavlink_message_t* msg, mavlink_${name_lower}_t* ${name_lower}) +{ +#if MAVLINK_NEED_BYTE_SWAP +${{ordered_fields: ${decode_left}mavlink_msg_${name_lower}_get_${name}(msg${decode_right}); +}} +#else + memcpy(${name_lower}, _MAV_PAYLOAD(msg), ${wire_length}); +#endif +} +''', m) + f.close() + + +def generate_testsuite_h(directory, xml): + '''generate testsuite.h per XML file''' + f = open(os.path.join(directory, "testsuite.h"), mode='w') + t.write(f, ''' +/** @file + * @brief MAVLink comm protocol testsuite generated from ${basename}.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef ${basename_upper}_TESTSUITE_H +#define ${basename_upper}_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +${{include_list:static void mavlink_test_${base}(uint8_t, uint8_t, mavlink_message_t *last_msg); +}} +static void mavlink_test_${basename}(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +${{include_list: mavlink_test_${base}(system_id, component_id, last_msg); +}} + mavlink_test_${basename}(system_id, component_id, last_msg); +} +#endif + +${{include_list:#include "../${base}/testsuite.h" +}} + +${{message: +static void mavlink_test_${name_lower}(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_${name_lower}_t packet_in = { + ${{ordered_fields:${c_test_value}, + }}}; + mavlink_${name_lower}_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + ${{scalar_fields: packet1.${name} = packet_in.${name}; + }} + ${{array_fields: mav_array_memcpy(packet1.${name}, packet_in.${name}, sizeof(${type})*${array_length}); + }} + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_${name_lower}_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_${name_lower}_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_${name_lower}_pack(system_id, component_id, &msg ${{arg_fields:, packet1.${name} }}); + mavlink_msg_${name_lower}_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_${name_lower}_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg ${{arg_fields:, packet1.${name} }}); + mavlink_msg_${name_lower}_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i