MAVLink: add support for hasLocation

ExtLibs/Mavlink/MavlinkParse.cs

@@ -55,6 +55,13 @@ public Description(string desc)
         public string Text { get; set; }
     }
 
+    public class hasLocation : Attribute
+    {
+        public hasLocation()
+        {
+        }
+    }
+
     public class MavlinkParse
     {
         public int packetcount = 0;

ExtLibs/Mavlink/message_definitions/ardupilotmega.xml

@@ -5,6 +5,7 @@
   <include>uAvionix.xml</include>
   <include>icarous.xml</include>
   <include>loweheiser.xml</include>
+  <include>cubepilot.xml</include>
   <dialect>2</dialect>
   <!-- Note that ArduPilot-specific messages should use the command id range from 150 to 250, to leave plenty of room for growth of common.xml If you prototype a message here, then you should consider if it is general enough to move into common.xml later -->
   <enums>
@@ -288,8 +289,8 @@
         <param index="2" label="timeout" units="s">timeout for operation in seconds. Zero means no timeout (0 to 255)</param>
         <param index="3" label="arg1">argument1.</param>
         <param index="4" label="arg2">argument2.</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
+        <param index="5" label="arg3">argument3.</param>
+        <param index="6" label="arg4">argument4.</param>
         <param index="7">Empty</param>
       </entry>
       <entry value="42703" name="MAV_CMD_NAV_ATTITUDE_TIME" hasLocation="false" isDestination="false">

ExtLibs/Mavlink/message_definitions/cubepilot.xml

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<mavlink>
+  <!-- Cubepilot contact info:                                              -->
+  <!-- company URL: http://www.cubepilot.com                                -->
+  <!-- email contact: siddharth@cubepilot.com or michael@cubepilot.com      -->
+  <!-- mavlink ID range: 50000 - 50099                                      -->
+  <include>common.xml</include>
+  <messages>
+    <message id="50001" name="CUBEPILOT_RAW_RC">
+      <description>Raw RC Data</description>
+      <field type="uint8_t[32]" name="rc_raw"/>
+    </message>
+    <message id="50002" name="HERELINK_VIDEO_STREAM_INFORMATION">
+      <description>Information about video stream</description>
+      <field type="uint8_t" name="camera_id">Video Stream ID (1 for first, 2 for second, etc.)</field>
+      <field type="uint8_t" name="status">Number of streams available.</field>
+      <field type="float" name="framerate" units="Hz">Frame rate.</field>
+      <field type="uint16_t" name="resolution_h" units="pix">Horizontal resolution.</field>
+      <field type="uint16_t" name="resolution_v" units="pix">Vertical resolution.</field>
+      <field type="uint32_t" name="bitrate" units="bits/s">Bit rate.</field>
+      <field type="uint16_t" name="rotation" units="deg">Video image rotation clockwise.</field>
+      <field type="char[230]" name="uri">Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to).</field>
+    </message>
+    <message id="50003" name="HERELINK_TELEM">
+      <description>Herelink Telemetry</description>
+      <field type="uint8_t" name="rssi"/>
+      <field type="int16_t" name="snr"/>
+      <field type="uint32_t" name="rf_freq"/>
+      <field type="uint32_t" name="link_bw"/>
+      <field type="uint32_t" name="link_rate"/>
+      <field type="int16_t" name="cpu_temp"/>
+      <field type="int16_t" name="board_temp"/>
+    </message>
+    <message id="50004" name="CUBEPILOT_FIRMWARE_UPDATE_START">
+      <description>Start firmware update with encapsulated data.</description>
+      <field type="uint8_t" name="target_system">System ID.</field>
+      <field type="uint8_t" name="target_component">Component ID.</field>
+      <field type="uint32_t" name="size" units="bytes">FW Size.</field>
+      <field type="uint32_t" name="crc">FW CRC.</field>
+    </message>
+    <message id="50005" name="CUBEPILOT_FIRMWARE_UPDATE_RESP">
+      <description>offset response to encapsulated data.</description>
+      <field type="uint8_t" name="target_system">System ID.</field>
+      <field type="uint8_t" name="target_component">Component ID.</field>
+      <field type="uint32_t" name="offset" units="bytes">FW Offset.</field>
+    </message>
+  </messages>
+</mavlink>
+

ExtLibs/Mavlink/message_definitions/offspec.xml

@@ -17,18 +17,6 @@
 			<field type="uint16_t" name="rotation" units="deg">Video image rotation clockwise.</field>
 			<field type="char[230]" name="uri">Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to).</field>
 		</message>
-		<message id="50003" name="HERELINK_TELEM">
-			<field type="uint8_t" name="rssi"></field>
-			<field type="int16_t" name="snr"></field>
-			<field type="uint32_t" name="rf_freq"></field>
-			<field type="uint32_t" name="link_bw"></field>
-			<field type="uint32_t" name="link_rate"></field>
-			<field type="int16_t" name="cpu_temp"></field>
-			<field type="int16_t" name="board_temp"></field>
-		</message>
-		<message id="50001" name="RAW_RC">
-			<field type="uint8_t[32]" name="rc_raw"></field>
-		</message>
 	</messages>
 	<enums>
 

ExtLibs/Mavlink/pymavlink/generator/CS/MavlinkParse.cs

@@ -55,6 +55,13 @@ public Description(string desc)
         public string Text { get; set; }
     }
 
+    public class hasLocation : Attribute
+    {
+        public hasLocation()
+        {
+        }
+    }
+
     public class MavlinkParse
     {
         public int packetcount = 0;

ExtLibs/Mavlink/pymavlink/generator/mavgen_cs.py

@@ -214,6 +214,12 @@ def generate_message_enums(f, xml):
             if hasattr(fe, "deprecated") and fe.deprecated is True:
                 fe.name = '''[Obsolete]
         %s''' % fe.name
+            if fe.has_location is True:
+                fe.name = '''[hasLocation()]
+        %s''' % fe.name
+            if hasattr(fe, "isDestination") and fe.isDestination is True:
+                fe.name = '''[isDestination()]
+        %s''' % fe.name
 
     t.write(f, '''
     ${{enum:

ExtLibs/Mavlink/pymavlink/generator/mavparse.py

@@ -33,7 +33,7 @@ def __str__(self):
         return self.message
 
 class MAVField(object):
-    def __init__(self, name, type, print_format, xml, description='', enum='', display='', units=''):
+    def __init__(self, name, type, print_format, xml, description='', enum='', display='', units='', instance=False):
         self.name = name
         self.name_upper = name.upper()
         self.description = description
@@ -44,6 +44,7 @@ def __init__(self, name, type, print_format, xml, description='', enum='', displ
         self.omit_arg = False
         self.const_value = None
         self.print_format = print_format
+        self.instance = instance
         lengths = {
         'float'    : 4,
         'double'   : 8,
@@ -131,6 +132,7 @@ def __init__(self, name, id, linenumber, description=''):
         self.fields = []
         self.fieldnames = []
         self.extensions_start = None
+        self.needs_pack = False
 
     def base_fields(self):
         '''return number of non-extended fields'''
@@ -161,7 +163,7 @@ def set_description(self, description):
             self.description = description
 
 class MAVEnumEntry(object):
-    def __init__(self, name, value, description='', end_marker=False, autovalue=False, origin_file='', origin_line=0):
+    def __init__(self, name, value, description='', end_marker=False, autovalue=False, origin_file='', origin_line=0, has_location=False):
         self.name = name
         self.value = value
         self.description = description
@@ -170,15 +172,17 @@ def __init__(self, name, value, description='', end_marker=False, autovalue=Fals
         self.autovalue = autovalue  # True if value was *not* specified in XML
         self.origin_file = origin_file
         self.origin_line = origin_line
+        self.has_location = has_location
 
 class MAVEnum(object):
-    def __init__(self, name, linenumber, description=''):
+    def __init__(self, name, linenumber, description='', bitmask=False):
         self.name = name
         self.description = description
         self.entry = []
         self.start_value = None
         self.highest_value = 0
         self.linenumber = linenumber
+        self.bitmask = bitmask
 
 class MAVXML(object):
     '''parse a mavlink XML file'''
@@ -224,7 +228,7 @@ def __init__(self, filename, wire_protocol_version=PROTOCOL_0_9):
             self.allow_extensions = True
         else:
             print("Unknown wire protocol version")
-            print("Available versions are: %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0, PROTOCOL_2_0))
+            print("Available versions are: %s %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0, PROTOCOL_2_0))
             raise MAVParseError('Unknown MAVLink wire protocol version %s' % wire_protocol_version)
 
         in_element_list = []
@@ -252,12 +256,14 @@ def start_element(name, attrs):
                 units = attrs.get('units', '')
                 if units:
                     units = '[' + units + ']'
-                new_field = MAVField(attrs['name'], attrs['type'], print_format, self, enum=enum, display=display, units=units)
+                instance = attrs.get('instance', False)
+                new_field = MAVField(attrs['name'], attrs['type'], print_format, self, enum=enum, display=display, units=units, instance=instance)
                 if self.message[-1].extensions_start is None or self.allow_extensions:
                     self.message[-1].fields.append(new_field)
             elif in_element == "mavlink.enums.enum":
                 check_attrs(attrs, ['name'], 'enum')
-                self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber))
+                bitmask = 'bitmask' in attrs and attrs['bitmask'] == 'true'
+                self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber, bitmask=bitmask))
             elif in_element == "mavlink.enums.enum.entry":
                 check_attrs(attrs, ['name'], 'enum entry')
                 # determine value and if it was automatically assigned (for possible merging later)
@@ -273,8 +279,15 @@ def start_element(name, attrs):
                 # check highest value
                 if (value > self.enum[-1].highest_value):
                     self.enum[-1].highest_value = value
+                has_location = attrs.get('hasLocation', False)
+                if has_location == 'true':
+                    has_location = True
+                elif has_location == 'false':
+                    has_location = False
+                if type(has_location) != bool:
+                    raise MAVParseError("invalid has_location value %s" % has_location)
                 # append the new entry
-                self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value, '', False, autovalue, self.filename, p.CurrentLineNumber))
+                self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value, '', False, autovalue, self.filename, p.CurrentLineNumber, has_location=has_location))
             elif in_element == "mavlink.enums.enum.entry.param":
                 check_attrs(attrs, ['index'], 'enum param')
                 self.enum[-1].entry[-1].param.append(
@@ -311,7 +324,7 @@ def char_data(data):
             elif in_element == "mavlink.enums.enum.entry.description":
                 self.enum[-1].entry[-1].description += data
             elif in_element == "mavlink.enums.enum.entry.param":
-                self.enum[-1].entry[-1].param[-1].set_description(data.strip())
+                self.enum[-1].entry[-1].param[-1].description += data
             elif in_element == "mavlink.version":
                 self.version = int(data)
             elif in_element == "mavlink.include":
@@ -379,6 +392,7 @@ def char_data(data):
             m.message_flags = 0
             m.target_system_ofs = 0
             m.target_component_ofs = 0
+            m.field_offsets = {}
 
             if self.sort_fields:
                 # when we have extensions we only sort up to the first extended field
@@ -402,7 +416,14 @@ def char_data(data):
             for i in range(len(m.ordered_fields)):
                 f = m.ordered_fields[i]
                 f.wire_offset = m.wire_length
+                m.field_offsets[f.name] = f.wire_offset
                 m.wire_length += f.wire_length
+                field_el_length = f.wire_length
+                if f.array_length > 1:
+                    field_el_length = f.wire_length / f.array_length
+                if f.wire_offset % field_el_length != 0:
+                    # misaligned field, structure will need packing in C
+                    m.needs_pack = True
                 if m.extensions_start is None or i < m.extensions_start:
                     m.wire_min_length = m.wire_length
                 m.ordered_fieldnames.append(f.name)
@@ -419,7 +440,7 @@ def char_data(data):
                     m.target_component_ofs = f.wire_offset
             m.num_fields = len(m.fieldnames)
             if m.num_fields > 64:
-                raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % (
+                raise MAVParseError("num_fields=%u : Maximum number of field names allowed is %u" % (
                     m.num_fields, 64))
             m.crc_extra = message_checksum(m)
 
@@ -435,16 +456,13 @@ def char_data(data):
             if m.wire_length > self.largest_payload:
                 self.largest_payload = m.wire_length
 
-            if m.wire_length+8 > 64:
-                print("Note: message %s is longer than 64 bytes long (%u bytes), which can cause fragmentation since many radio modems use 64 bytes as maximum air transfer unit." % (m.name, m.wire_length+8))
-
     def __str__(self):
         return "MAVXML for %s from %s (%u message, %u enums)" % (
             self.basename, self.filename, len(self.message), len(self.enum))
 
 
 def message_checksum(msg):
-    '''calculate a 8-bit checksum of the key fields of a message, so we
+    '''calculate CRC-16/MCRF4XX checksum of the key fields of a message, so we
        can detect incompatible XML changes'''
     from .mavcrc import x25crc
     crc = x25crc()

ExtLibs/Mavlink/updatexmls.bat

@@ -11,4 +11,6 @@ python -c "import urllib.request; print(urllib.request.urlopen('https://github.c
 
 python -c "import urllib.request; print(urllib.request.urlopen('https://github.com/ArduPilot/mavlink/raw/master/message_definitions/v1.0/loweheiser.xml').read().decode('utf-8') )" > message_definitions\loweheiser.xml
 
+python -c "import urllib.request; print(urllib.request.urlopen('https://github.com/ArduPilot/mavlink/raw/master/message_definitions/v1.0/cubepilot.xml').read().decode('utf-8') )" > message_definitions\cubepilot.xml
+
 pause