Skip to content

Fixed several issues in the spin2 generator implmentation. #1083

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
tridge merged 1 commit into from
Aug 1, 2025
Merged

Fixed several issues in the spin2 generator implmentation. #1083

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
118 changes: 74 additions & 44 deletions generator/mavgen_spin2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,10 @@ def generate_preamble(outf, msgs, basename, args, xml):
WORD crc
BYTE tmp1, tmp2

pub get_crc():result
result := crc
return

pub make_crc(msgadr,len,CRC_EXTRA)
crc:=$FFFF
byte[msgadr+len-2]:= CRC_EXTRA & $FF ' stick the crcextra in the first byte
Expand Down Expand Up @@ -201,39 +205,59 @@ def generate_message_defs(outf, msgs, enums):
field.name = field.name.replace("A","").replace("E","").replace("I","").replace("O","").replace("U","")
field.name = field.name.lower()
if len(field.name) > 30:
field.name = field.name[0:30]
if field.type == "char": # I'm an array of chars let's just allocate the memory
m.spin_fields.append("BYTE " + field.name + "[" + str(field.array_length) + "]")
m.spin_names.append(field.name)
elif field.type == "uint8_t" or field.type == "int8_t":
m.spin_fields.append("BYTE " + field.name)
m.spin_names.append(field.name)
field.name = field.name[0:30]
if field.type == "uint8_t" or field.type == "int8_t" or field.type.startswith("char"):
if(field.array_length > 0):
m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
m.spin_fields.append("BYTE " + field.name + "[" + str(field.array_length) + "]")
else:
m.spin_names.append(field.name)
m.spin_fields.append("BYTE " + field.name)
elif field.type == "uint16_t" or field.type == "int16_t":
m.spin_fields.append("WORD " + field.name)
m.spin_names.append(field.name)
elif field.type == "uint32_t" or field.type == "int32_t":
m.spin_names.append(field.name)
m.spin_fields.append(field.name) #default is long save progmem
if(field.array_length > 0):
m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
m.spin_fields.append("WORD " + field.name + "[" + str(field.array_length) + "]")
else:
m.spin_names.append(field.name)
m.spin_fields.append("WORD " + field.name)
elif field.type == "uint32_t" or field.type == "int32_t" or field.type == "float":
if(field.array_length > 0):
m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
m.spin_fields.append(field.name + "[" + str(field.array_length) + "]")
else:
m.spin_names.append(field.name)
m.spin_fields.append(field.name)
elif field.type == "uint64_t" or field.type == "int64_t":
m.spin_fields.append("int64 " + field.name) # we need a structured type for this
m.spin_names.append("int64 " + field.name)
elif field.type == "int16_t": # signed
m.spin_fields.append("WORD " + field.name)
m.spin_names.append(field.name)
elif field.type == "float":
m.spin_fields.append(field.name)
m.spin_names.append(field.name)
if(field.array_length > 0):
m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
m.spin_fields.append("int64 " + field.name + "[" + str(field.array_length) + "]")
else:
m.spin_names.append("int64 " + field.name)
m.spin_fields.append("int64 " + field.name)
elif field.type == "double":
m.spin_fields.append("double " + field.name)
m.spin_names.append("double " + field.name)
if(field.array_length > 0):
m.spin_names.append(field.name) # array pointer takes precedence over struct pointer, we just receive a long
m.spin_fields.append("double " + field.name + "[" + str(field.array_length) + "]")
else:
m.spin_names.append("double " + field.name)
m.spin_fields.append("double " + field.name)

else:
if field.array_length > 0:
print("APPENDING FIELD WITH ARRAY LENGTH")
m.spin_fields.append(field.type + " " + field.name + "[" + str(field.array_length) + "]") # I'm a struct array
if field.array_length > 0:
m.spin_names.append(field.name) # I'm an array of unknown structs
m.spin_fields.append(field.type + field.name + "[" + str(field.array_length) + "]")
else:
m.spin_fields.append(field.type + " " + field.name) # I'm a struct
m.spin_names.append(field.name)
m.init_fields.append("msg.%s := %s" % (field.name, field.name))
m.spin_fields.append(field.type + " " + field.name) # I'm an unknown struct
m.spin_names.append(field.type + " " + field.name)
if(field.array_length > 0):
if(field.type == "double"):
m.init_fields.append("BYTEMOVE(@msg.%s, %s, %s*sizeof(double))" % (field.name, field.name, field.array_length))
elif(field.type == "uint64_t" or field.type == "int64_t"):
m.init_fields.append("BYTEMOVE(@msg.%s, %s, %s*sizeof(int64)" % (field.name, field.name, field.array_length))
else:
m.init_fields.append("BYTEMOVE(@msg.%s, %s, %s)" % (field.name, field.name, field.array_length))
else:
m.init_fields.append("msg.%s := %s" % (field.name, field.name))
t.write(
outf,
'''
Expand All @@ -246,20 +270,21 @@ def generate_message_defs(outf, msgs, enums):
${cleanname}_crcx = ${crc_extra}
STRUCT ${classname} (${spin_fields})

PUB ${cleanname}_pack(${field_names}): siz | ${classname} msg
PUB ${cleanname}_pack(${field_names}, ^${classname} msg): siz
BYTEFILL(@payload_buf, 0, MAVLINK_PAYLOAD_SIZE)
${init_fields}
' build payload
BYTEMOVE(@payload_buf, @msg, sizeof(msg))
siz := sizeof(msg)
BYTEMOVE(@payload_buf, @msg, siz)


PUB ${cleanname}_send(${field_names}): siz | ${classname} msg
BYTEFILL(@payload_buf, 0, MAVLINK_PAYLOAD_SIZE)
${init_fields}
${init_fields}
' build payload
BYTEMOVE(@payload_buf, @msg, sizeof(msg))
send_mavlink(${cleanname}_id, srcSystemId, srcComponentId, ${cleanname}_crcx, sizeof(msg), False)
siz := sizeof(msg)
BYTEMOVE(@payload_buf, @msg, siz)
send_mavlink(${cleanname}_id, srcSystemId, srcComponentId, ${cleanname}_crcx, siz, False)

''',
{
Expand Down Expand Up @@ -537,10 +562,10 @@ def generate_mavlink_parser(outf, msgs, xml):
parse_state := PARSE_STATE_GOT_PAYLOAD
PARSE_STATE_GOT_PAYLOAD:
' don't handle the CRC just read it, verify it in handling steps
byte[@inPacket.checksum +1] := c
byte[@inPacket.checksum] := c
parse_state := PARSE_STATE_GOT_CRC1
PARSE_STATE_GOT_CRC1:
byte[@inPacket.checksum] := c
byte[@inPacket.checksum + 1] := c
parse_state := PARSE_STATE_GOT_CRC2 ' done; this is our terminator state
total_packets_sent++
PARSE_STATE_GOT_CRC2:
Expand Down Expand Up @@ -636,23 +661,28 @@ def generate_case(outf, msgs, dialect):
Handles an incoming MAVLink message.
}}

PUB checkCrc(WORD crc, crcextra): result | newcrc
mavlink.make_crc(@msg, msg.len+12, crcextra)
newcrc := mavlink.get_crc
if (crc <> newcrc)
PUB checkCrc(crcextra, ^mavlink.MAVLink msg): result | newcrc
mavlink.make_crc([msg], msg.len+12, crcextra)
newcrc := mavlink.get_crc()
if (msg.checksum <> newcrc)
return FALSE
else
return TRUE

PUB crcError()
' TODO: Handle CRC errors for your solution

PUB handleMessage(^mavlink.MAVLink packet) | msgid
' little endian
msgid := packet.msgid_h << 16
msgid |= packet.msgid_m << 8
msgid |= packet.msgid

CASE_FAST msgid
' Can't use CASE_FAST; ID values have to be within 255 of each other
' maybe use a compare and two CASE statements in a flight controller implementation but that is beyond the scope of pymavgen
CASE msgid
mavlink.MSG_ID_MANUAL_CONTROL: ' highest priority is manual control and mode messages
handle_manual_control(@packet)
hdl_manual_control(@packet)
''',{"DIALECT": dialect}, )
for m in msgs:
if (m.name == "MANUAL_CONTROL" or m.name == "HEARTBEAT"): # skip manual control and heartbeat; manual control should be first and heartbeat last.
Expand All @@ -661,7 +691,7 @@ def generate_case(outf, msgs, dialect):
outf,
'''
mavlink.${cleanname}_id:
if(checkCrc(mavlink.${cleanname}_crcx)
if(checkCrc(mavlink.${cleanname}_crcx, @packet))
hdl_${cleanname}(@packet)
else
crcError()
Expand All @@ -681,7 +711,7 @@ def generate_case(outf, msgs, dialect):
outf,
'''
mavlink.MSG_ID_HEARTBEAT:
handle_heartbeat(@packet)
hdl_heartbeat(@packet)

''')
def generate_handler(outf, msgs):
Expand All @@ -693,7 +723,7 @@ def generate_handler(outf, msgs):
${spin_fields}
}}
PUB hdl_${cleanname}(^mavlink.MAVLink packet) | mavlink.${classname} msg
BYTEMOVE(@msg, @packet.payload, sizeof(msg)
BYTEMOVE(@msg, @packet.payload, sizeof(msg))
' TODO: implement handler

''',
Expand Down