Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 11 additions & 9 deletions generator/mavgen_c.py
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,7 @@ def generate_message_h(directory, m):
mavlink_${name_lower}_t packet;
${{scalar_fields: packet.${name} = ${putname};
}}
${{array_fields: mav_array_assign_${type}(packet.${name}, ${name}, ${array_length});
${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
}}
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_${name}_LEN);
#endif
Expand Down Expand Up @@ -299,7 +299,7 @@ def generate_message_h(directory, m):
mavlink_${name_lower}_t packet;
${{scalar_fields: packet.${name} = ${putname};
}}
${{array_fields: mav_array_assign_${type}(packet.${name}, ${name}, ${array_length});
${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
}}
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_${name}_LEN);
#endif
Expand Down Expand Up @@ -371,7 +371,7 @@ def generate_message_h(directory, m):
mavlink_${name_lower}_t packet;
${{scalar_fields: packet.${name} = ${putname};
}}
${{array_fields: mav_array_assign_${type}(packet.${name}, ${name}, ${array_length});
${{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, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
#endif
Expand All @@ -393,7 +393,7 @@ def generate_message_h(directory, m):

#if MAVLINK_MSG_ID_${name}_LEN <= MAVLINK_MAX_PAYLOAD_LEN
/*
This variant of _send() can be used to save stack space by reusing
This variant of _send() can be used to save stack space by re-using
memory from the receive buffer. The caller provides a
mavlink_message_t which is the size of a full mavlink message. This
is usually the receive buffer for the channel, and allows a reply to an
Expand All @@ -412,7 +412,7 @@ def generate_message_h(directory, m):
mavlink_${name_lower}_t *packet = (mavlink_${name_lower}_t *)msgbuf;
${{scalar_fields: packet->${name} = ${putname};
}}
${{array_fields: mav_array_assign_${type}(packet->${name}, ${name}, ${array_length});
${{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, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
#endif
Expand Down Expand Up @@ -588,7 +588,7 @@ class mav_include(object):
def __init__(self, base):
self.base = base

def generate_one(basename, xml):
def generate_one(basename, xml, opts=None):
'''generate headers for one XML file'''

directory = os.path.join(basename, xml.basename)
Expand Down Expand Up @@ -723,7 +723,9 @@ def generate_one(basename, xml):
f.c_test_value = "%sLL" % f.test_value
else:
f.c_test_value = f.test_value
if m.needs_pack:
# Check if forced struct packing is enabled (useful for platforms like ESP32)
force_pack = getattr(opts, 'force_pack', False)
if m.needs_pack or force_pack:
m.MAVPACKED_START = "MAVPACKED("
m.MAVPACKED_END = ")"
else:
Expand Down Expand Up @@ -755,11 +757,11 @@ def generate_one(basename, xml):
generate_testsuite_h(directory, xml)


def generate(basename, xml_list):
def generate(basename, xml_list, opts=None):
'''generate complete MAVLink C implemenation'''

for idx in range(len(xml_list)):
xml = xml_list[idx]
xml.xml_hash = hash(xml.basename)
generate_one(basename, xml)
generate_one(basename, xml, opts)
copy_fixed_headers(basename, xml_list[0])
1 change: 1 addition & 0 deletions tools/mavgen.py
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
parser.add_argument("--wire-protocol", choices=[mavparse.PROTOCOL_0_9, mavparse.PROTOCOL_1_0, mavparse.PROTOCOL_2_0], default=mavgen.DEFAULT_WIRE_PROTOCOL, help="MAVLink protocol version. [default: %(default)s]")
parser.add_argument("--no-validate", action="store_false", dest="validate", default=mavgen.DEFAULT_VALIDATE, help="Do not perform XML validation. Can speed up code generation if XML files are known to be correct.")
parser.add_argument("--strict-units", action="store_true", dest="strict_units", default=mavgen.DEFAULT_STRICT_UNITS, help="Perform validation of units attributes.")
parser.add_argument("--force-pack", action="store_true", dest="force_pack", default=False, help="Force struct packing on all messages (useful for platforms with different alignment like ESP32).")
parser.add_argument("definitions", metavar="XML", nargs="+", help="MAVLink definitions")
args = parser.parse_args()

Expand Down