Skip to content

Commit

Permalink
mavgen_c: added XXX_encode_status bindings
Browse files Browse the repository at this point in the history 
this allows for sending messages with correctly incrementing sequence
numbers without assuming channel 0 and without global status methods
  • Loading branch information
@tridge
tridge committed Sep 29, 2023
1 parent ab5a4c2 commit 19e385c
Show file tree
Hide file tree
Showing 5 changed files with 101 additions and 1 deletion.
2 changes: 2 additions & 0 deletions generator/C/include_v0.9/protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@
/* always include the prototypes to ensure we don't get out of sync */
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
Expand Down
42 changes: 41 additions & 1 deletion generator/C/include_v1.0/mavlink_helpers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,46 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
}

/**
* @brief Finalize a MAVLink message with channel assignment
*
* This function calculates the checksum and sets length and aircraft id correctly.
* It assumes that the message id and the payload are already correctly set. This function
* can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
* instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
*
* @param msg Message to finalize
* @param system_id Id of the sending (this) system, 1-127
* @param length Message length
*/
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t length)
#endif
{
// This is only used for the v2 protocol and we silence it here
(void)min_length;
// This code part is the same for all messages;
msg->magic = MAVLINK_STX;
msg->len = length;
msg->sysid = system_id;
msg->compid = component_id;
msg->seq = status->current_tx_seq;
status->current_tx_seq = status->current_tx_seq+1;
msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &msg->checksum);
#endif
mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);

return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}

/**
* @brief Finalize a MAVLink message with channel assignment
*
Expand All @@ -72,7 +112,7 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
uint8_t chan, uint8_t length)
#endif
{
// This is only used for the v2 protocol and we silence it here
// This is only used for the v2 protocol and we silence it here
(void)min_length;
// This code part is the same for all messages;
msg->magic = MAVLINK_STX;
Expand Down
4 changes: 4 additions & 0 deletions generator/C/include_v1.0/protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@
#endif
MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
Expand All @@ -54,6 +56,8 @@
uint8_t min_length, uint8_t length, uint8_t crc_extra);
#endif
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t min_length, uint8_t length);
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
Expand Down
2 changes: 2 additions & 0 deletions generator/C/include_v2.0/protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,8 @@
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
#endif
MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
Expand Down
52 changes: 52 additions & 0 deletions generator/mavgen_c.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -241,6 +241,44 @@ def generate_message_h(directory, m):
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
}
/**
* @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 status MAVLink status structure
* @param msg The MAVLink message to compress the data into
*
${{arg_fields: * @param ${name} ${units} ${description}
}}
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_${name_lower}_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_${name}_LEN];
${{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, MAVLINK_MSG_ID_${name}_LEN);
#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, MAVLINK_MSG_ID_${name}_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_${name};
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
#else
return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN);
#endif
}
/**
* @brief Pack a ${name_lower} message on a channel
* @param system_id ID of this system
Expand Down Expand Up @@ -302,6 +340,20 @@ def generate_message_h(directory, m):
return mavlink_msg_${name_lower}_pack_chan(system_id, component_id, chan, msg,${{arg_fields: ${name_lower}->${name},}});
}
/**
* @brief Encode a ${name_lower} struct with provided status structure
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param status MAVLink status structure
* @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_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_${name_lower}_t* ${name_lower})
{
return mavlink_msg_${name_lower}_pack_status(system_id, component_id, _status, msg, ${{arg_fields: ${name_lower}->${name},}});
}
/**
* @brief Send a ${name_lower} message
* @param chan MAVLink channel to send the message
Expand Down

0 comments on commit 19e385c

Please sign in to comment.