diff options
author | David Sidrane <david_s5@nscdg.com> | 2015-04-21 15:14:38 -1000 |
---|---|---|
committer | David Sidrane <david_s5@nscdg.com> | 2015-04-22 02:30:14 -1000 |
commit | 3ce416fa8e5ed546590f5161693132ace738fcba (patch) | |
tree | 522180d5c3d2de9dfe61ca4b6bf6b6ba3cd47ac1 | |
parent | c5edd2e80eca047ce5e883d94ef0d458eef8c359 (diff) | |
download | px4-firmware-3ce416fa8e5ed546590f5161693132ace738fcba.tar.gz px4-firmware-3ce416fa8e5ed546590f5161693132ace738fcba.tar.bz2 px4-firmware-3ce416fa8e5ed546590f5161693132ace738fcba.zip |
Documnetation pass
-rw-r--r-- | src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c | 793 | ||||
-rw-r--r-- | src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h | 385 |
2 files changed, 853 insertions, 325 deletions
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c index edc6135c8..0d8a46193 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.c @@ -1,3 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer <ben_dyer@mac.com> + * Pavel Kirienko <pavel.kirienko@zubax.com> + * David Sidrane <david_s5@nscdg.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + #include <nuttx/config.h> #include "board_config.h" @@ -12,58 +52,269 @@ #include "driver.h" #include "crc.h" +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ #define CAN_REQUEST_TIMEOUT 1000 -static void uavcan_tx_multiframe_( - uavcan_frame_id_t *frame_id, - uint8_t dest_node_id, - size_t message_length, - const uint8_t *message, - uint16_t initial_crc, - uint8_t mailbox -); -static can_error_t uavcan_rx_multiframe_( - uint8_t node_id, - uavcan_frame_id_t *frame_id, - size_t *message_length, - uint8_t *message, - uint16_t initial_crc, - uint32_t timeout_cycles -); - - -size_t uavcan_pack_nodestatus( - uint8_t *data, - const uavcan_nodestatus_t *payload -) { +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ +/**************************************************************************** + * Name: uavcan_tx_multiframe_ + ****************************************************************************/ +static void uavcan_tx_multiframe_(uavcan_frame_id_t *frame_id, + uint8_t dest_node_id, + size_t message_length, + const uint8_t *message, + uint16_t initial_crc, + uint8_t mailbox) +{ + uint32_t i, m; + uint16_t frame_crc; + uint8_t payload[8]; + + /* Calculate the message CRC */ + frame_crc = crc16_signature(initial_crc, message_length, message); + + /* Ensure message ID has the frame details zeroed */ + frame_id->last_frame = 0u; + frame_id->frame_index = 0u; + + /* + Send the message -- only prepend CRC if the message will not fit within a + single frame + */ + payload[0] = dest_node_id; + m = 1u; + if (message_length > 7u) { + payload[m++] = (uint8_t)frame_crc; + payload[m++] = (uint8_t)(frame_crc >> 8u); + } + for (i = 0u; i < message_length; i++) { + payload[m++] = message[i]; + if (i == message_length - 1u) { + break; + } else if (m == 8u) { + can_tx(uavcan_make_message_id(frame_id), 8u, payload, mailbox); + frame_id->frame_index++; + payload[0] = dest_node_id; + m = 1u; + } + } + + /* Send the last (only?) frame */ + frame_id->last_frame = 1u; + can_tx(uavcan_make_message_id(frame_id), m, payload, mailbox); +} + +/**************************************************************************** + * Name: uavcan_rx_multiframe_ + ****************************************************************************/ + +static can_error_t uavcan_rx_multiframe_(uint8_t node_id, + uavcan_frame_id_t *frame_id, + size_t *message_length, + uint8_t *message, + uint16_t initial_crc, + uint32_t timeout_ms) +{ + uavcan_frame_id_t rx_id; + size_t rx_length; + size_t m; + size_t i; + uint32_t rx_message_id; + uint16_t calculated_crc; + uint16_t message_crc; + uint8_t payload[8]; + uint8_t got_frame; + uint8_t num_frames; + + bl_timer_id timer = timer_allocate(modeTimeout|modeStarted, timeout_ms, 0); + + num_frames = 0u; + rx_id.last_frame = 0u; + message_crc = 0u; + i = 0; + + do { + rx_message_id = 0u; + rx_length = 0u; + got_frame = can_rx(&rx_message_id, &rx_length, payload, MBAll); + if (!got_frame) { + continue; + } + + uavcan_parse_message_id(&rx_id, rx_message_id, frame_id->transfer_type); + + /* + Skip this frame if the source node ID is wrong, or if the data type or + transfer type do not match what we're expecting + */ + if ((frame_id->source_node_id != 0xFFu && + rx_id.source_node_id != frame_id->source_node_id) || + rx_id.transfer_type != frame_id->transfer_type || + rx_id.data_type_id != frame_id->data_type_id || + payload[0] != node_id) { + continue; + } + + /* + Get the CRC if this is the first frame of a multi-frame transfer. + + Also increase the timeout to UAVCAN_SERVICE_TIMEOUT_TICKS. + */ + if (rx_id.frame_index == 0u) { + num_frames = 0u; + frame_id->transfer_id = rx_id.transfer_id; + if (frame_id->source_node_id == 0xFFu) { + frame_id->source_node_id = rx_id.source_node_id; + } + } + + /* Skip if the transfer ID is wrong */ + if ((frame_id->transfer_id ^ rx_id.transfer_id) & 0x7u) { + continue; + } + + /* + Get the CRC and increase the service timeout if this is the first + frame + */ + if (num_frames == 0u && !rx_id.last_frame) { + timer_restart(timer, UAVCAN_SERVICE_TIMEOUT_MS); + message_crc = (uint16_t)(payload[1] | (payload[2] << 8u)); + m = 3u; + } else { + m = 1u; + } + + /* Copy message bytes to the response */ + for (; m < rx_length && i < *message_length; m++, i++) { + message[i] = payload[m]; + } + num_frames++; + + if (rx_id.last_frame) { + break; + } + } while (!timer_expired(timer)); + timer_free(timer); + if (!rx_id.last_frame) { + return CAN_ERROR; + } else { + /* Validate CRC */ + calculated_crc = crc16_signature(initial_crc, i, message); + + *message_length = i; + + if (num_frames == 1u || message_crc == calculated_crc) { + return CAN_OK; + } else { + return CAN_ERROR; + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: uavcan_pack_nodestatus + * + * Description: + * This function formats the data of a uavcan_nodestatus_t structure into + * an array of bytes. + * + * Input Parameters: + * data - The array of bytes to populate. + * message - The uavcan_nodestatus_t to pack into the data + * + * Returned value: + * Number of bytes written. + * + ****************************************************************************/ + +size_t uavcan_pack_nodestatus(uint8_t *data, + const uavcan_nodestatus_t *message) +{ /* No need to clear the top 4 bits of uptime_sec */ - data[0] = ((uint8_t*)&payload->uptime_sec)[0]; - data[1] = ((uint8_t*)&payload->uptime_sec)[1]; - data[2] = ((uint8_t*)&payload->uptime_sec)[2]; - data[3] = ((uint8_t*)&payload->uptime_sec)[3] | payload->status_code; - data[4] = ((uint8_t*)&payload->vendor_specific_status_code)[0]; - data[5] = ((uint8_t*)&payload->vendor_specific_status_code)[1]; + data[0] = ((uint8_t*)&message->uptime_sec)[0]; + data[1] = ((uint8_t*)&message->uptime_sec)[1]; + data[2] = ((uint8_t*)&message->uptime_sec)[2]; + data[3] = ((uint8_t*)&message->uptime_sec)[3] | message->status_code; + data[4] = ((uint8_t*)&message->vendor_specific_status_code)[0]; + data[5] = ((uint8_t*)&message->vendor_specific_status_code)[1]; return 6u; } -size_t uavcan_pack_logmessage( - uint8_t *data, - const uavcan_logmessage_t *payload -) { - data[0] = (uint8_t)(((payload->level & 0x7u) << 5u) | 4u); +/**************************************************************************** + * Name: uavcan_pack_logmessage + * + * Description: + * This function formats the data of a uavcan_logmessage_t structure into + * an array of bytes. + * + * Input Parameters: + * data - The array of bytes to populate. + * message - The uavcan_logmessage_t to pack into the data + * + * Returned value: + * Number of bytes written. + * + ****************************************************************************/ + +size_t uavcan_pack_logmessage(uint8_t *data, + const uavcan_logmessage_t *message) +{ + data[0] = (uint8_t)(((message->level & 0x7u) << 5u) | 4u); data[1] = UAVCAN_LOGMESSAGE_SOURCE_0; data[2] = UAVCAN_LOGMESSAGE_SOURCE_1; data[3] = UAVCAN_LOGMESSAGE_SOURCE_2; data[4] = UAVCAN_LOGMESSAGE_SOURCE_3; - data[5] = payload->message[0]; - data[6] = payload->message[1]; + data[5] = message->message[0]; + data[6] = message->message[1]; return 7u; } -uint32_t uavcan_make_message_id(const uavcan_frame_id_t *frame_id) { +/**************************************************************************** + * Name: uavcan_make_message_id + * + * Description: + * This function formats the data of a uavcan_frame_id_t structure into + * a unit32. + * + * Input Parameters: + * frame_id - The uavcan_frame_id_t to pack. + * + * Returned value: + * A unit32 that is the message id formed from packing the + * uavcan_frame_id_t. + * + ****************************************************************************/ + +uint32_t uavcan_make_message_id(const uavcan_frame_id_t *frame_id) +{ return (frame_id->transfer_id & 0x7u) | ((frame_id->last_frame ? 1u : 0u) << 3u) | ((frame_id->frame_index & 0x3Fu) << 4u) | @@ -72,7 +323,24 @@ uint32_t uavcan_make_message_id(const uavcan_frame_id_t *frame_id) { ((frame_id->data_type_id & 0x3FFu) << 19u); } - +/**************************************************************************** + * Name: uavcan_parse_message_id + * + * Description: + * This function formats the data of a uavcan message_id contained in a uint32 + * into uavcan_frame_id_t structure. + * + * Input Parameters: + * frame_id - A pointer to a uavcan_frame_id_t parse the message_id into. + * message_id - The message id to parse into the uavcan_frame_id_t + * expected_id -The expected uavcan data_type_id that has been parsed into + * frame_id's data_type_id field + * + * Returned value: + * The result of comparing the data_type_id to the expected_id + * Non Zero if they match otherwise zero. + * + ****************************************************************************/ int uavcan_parse_message_id(uavcan_frame_id_t *frame_id, uint32_t message_id, uint16_t expected_id) { @@ -85,43 +353,79 @@ int uavcan_parse_message_id(uavcan_frame_id_t *frame_id, uint32_t message_id, return expected_id == frame_id->data_type_id; } - -void uavcan_tx_nodestatus( - uint8_t node_id, - uint32_t uptime_sec, - uint8_t status_code -) { - uavcan_nodestatus_t message; - uavcan_frame_id_t frame_id; - uint8_t payload[8]; - size_t frame_len; - - frame_id.transfer_id = 0; - frame_id.last_frame = 1u; - frame_id.frame_index = 0; - frame_id.source_node_id = node_id; - frame_id.transfer_type = MESSAGE_BROADCAST; - frame_id.data_type_id = UAVCAN_NODESTATUS_DTID; - - message.uptime_sec = uptime_sec; - message.status_code = status_code; - message.vendor_specific_status_code = 0u; - frame_len = uavcan_pack_nodestatus(payload, &message); - - can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBNodeStatus); +/**************************************************************************** + * Name: uavcan_tx_nodestatus + * + * Description: + * This function sends a uavcan nodestatus message + * + * Input Parameters: + * node_id - This node's node id + * uptime_sec - This node's uptime in seconds. + * status_code - This node's current status code + * + * Returned value: + * None. + * + ****************************************************************************/ + +void uavcan_tx_nodestatus(uint8_t node_id, uint32_t uptime_sec, + uint8_t status_code) +{ + uavcan_nodestatus_t message; + uavcan_frame_id_t frame_id; + uint8_t payload[8]; + size_t frame_len; + + frame_id.transfer_id = 0; + frame_id.last_frame = 1u; + frame_id.frame_index = 0; + frame_id.source_node_id = node_id; + frame_id.transfer_type = MESSAGE_BROADCAST; + frame_id.data_type_id = UAVCAN_NODESTATUS_DTID; + + message.uptime_sec = uptime_sec; + message.status_code = status_code; + message.vendor_specific_status_code = 0u; + frame_len = uavcan_pack_nodestatus(payload, &message); + + can_tx(uavcan_make_message_id(&frame_id), frame_len, payload, MBNodeStatus); } - -void uavcan_tx_allocation_message( - uint8_t requested_node_id, - size_t unique_id_length, - const uint8_t *unique_id, - uint8_t unique_id_offset, - uint8_t transfer_id -) { +/**************************************************************************** + * Name: uavcan_tx_allocation_message + * + * Description: + * This function sends a uavcan allocation message. + * + * Input Parameters: + * requested_node_id - This node's preferred node id 0 for no preference. + * unique_id_length - This node's length of it's unique identifier. + * unique_id - A pointer to the bytes that represent unique + * identifier. + * unique_id_offset - The offset equal 0 or the number of bytes in the + * the last received message that matched the unique ID + * field. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message + * + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_allocation_message(uint8_t requested_node_id, + size_t unique_id_length, + const uint8_t *unique_id, + uint8_t unique_id_offset, + uint8_t transfer_id) +{ uint8_t payload[8]; uavcan_frame_id_t frame_id; - size_t i, max_offset, checksum; + size_t i; + size_t max_offset; + size_t checksum; max_offset = unique_id_offset + 7u; if (max_offset > unique_id_length) { @@ -146,20 +450,41 @@ void uavcan_tx_allocation_message( payload, MBAll); } - -void uavcan_tx_getnodeinfo_response( - uint8_t node_id, - uavcan_getnodeinfo_response_t *response, - uint8_t dest_node_id, - uint8_t transfer_id -) { +/**************************************************************************** + * Name: uavcan_tx_getnodeinfo_response + * + * Description: + * This function sends a uavcan getnodeinfo response to a getnodeinfo + * request message. + * + * Input Parameters: + * node_id - This node's node id + * response - A pointer to this node's uavcan_getnodeinfo_response_t + * response data. + * dest_node_id - The destination node id to send the message to. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_getnodeinfo_response(uint8_t node_id, + uavcan_getnodeinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id) +{ /* This sends via mailbox 1 because it's called from SysTick. It may also clobber response->name because it moves the name to the end of the COA. */ uint32_t i; uavcan_frame_id_t frame_id; - size_t fixed_length, contiguous_length, packet_length; + size_t fixed_length; + size_t contiguous_length; + size_t packet_length; fixed_length = 6u + sizeof(uavcan_softwareversion_t) + 2u + 16u + 1u; contiguous_length = fixed_length + @@ -182,11 +507,30 @@ void uavcan_tx_getnodeinfo_response( 1u); } -can_error_t uavcan_rx_beginfirmwareupdate_request( - uint8_t node_id, - uavcan_beginfirmwareupdate_request_t *request, - uavcan_frame_id_t *frame_id -) { +/**************************************************************************** + * Name: uavcan_rx_beginfirmwareupdate_request + * + * Description: + * This function attempts to receive a uavcan beginfirmwareupdate request + * message + * + * Input Parameters: + * node_id - This node's node id + * request - A pointer a uavcan_beginfirmwareupdate_request_t to + * receive the request data into. + * frame_id - A pointer to a uavcan_frame_id_t to return frame_id + * components in. + * + * + * Returned value: + * CAN_OK on Success and CAN_ERROR otherwise. + * + ****************************************************************************/ + +can_error_t uavcan_rx_beginfirmwareupdate_request(uint8_t node_id, + uavcan_beginfirmwareupdate_request_t *request, + uavcan_frame_id_t *frame_id) +{ size_t length; can_error_t status; @@ -208,13 +552,29 @@ can_error_t uavcan_rx_beginfirmwareupdate_request( } } - -void uavcan_tx_read_request( - uint8_t node_id, - const uavcan_read_request_t *request, - uint8_t dest_node_id, - uint8_t transfer_id -) { +/**************************************************************************** + * Name: uavcan_tx_read_request + * + * Description: + * This function sends a uavcan read request message. + * + * Input Parameters: + * node_id - This node's node id + * request - A pointer a uavcan_read_request_t to + * send. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_read_request(uint8_t node_id, + const uavcan_read_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id) +{ uavcan_frame_id_t frame_id; /* Set up the message ID */ @@ -228,13 +588,33 @@ void uavcan_tx_read_request( } -can_error_t uavcan_rx_read_response( - uint8_t node_id, - uavcan_read_response_t *response, - uint8_t dest_node_id, - uint8_t transfer_id, - uint32_t timeout_ticks -) { +/**************************************************************************** + * Name: uavcan_rx_read_response + * + * Description: + * This function attempts to receive a uavcan read response message. + * + * Input Parameters: + * node_id - This node's node id + * response - A pointer a uavcan_read_response_t to receive the + * response data into. + * dest_node_id - The remote node id to expect the message from. + * transfer_id - The expected transfer_id used to correlate this response + * message to the transmitted request message. + * timeout_ms - The number of milliseconds to wait for a response + * + * + * Returned value: + * CAN_OK on Success and CAN_ERROR otherwise. + * + ****************************************************************************/ + +can_error_t uavcan_rx_read_response(uint8_t node_id, + uavcan_read_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ms) +{ uavcan_frame_id_t frame_id; size_t length; can_error_t status; @@ -247,7 +627,7 @@ can_error_t uavcan_rx_read_response( status = uavcan_rx_multiframe_(node_id, &frame_id, &length, (uint8_t*)response, UAVCAN_READ_CRC, - timeout_ticks); + timeout_ms); if (status == CAN_OK && length >= 2u) { response->data_length = (uint8_t)(length - 2u); @@ -257,13 +637,30 @@ can_error_t uavcan_rx_read_response( } } - -void uavcan_tx_getinfo_request( - uint8_t node_id, - const uavcan_getinfo_request_t *request, - uint8_t dest_node_id, - uint8_t transfer_id -) { +/**************************************************************************** + * Name: uavcan_tx_getinfo_request + * + * Description: + * This function sends a uavcan getinfo request message. + * + * Input Parameters: + * node_id - This node's node id + * request - A pointer a uavcan_getinfo_request_t to + * send. + * dest_node_id - The destination node id to send the message to. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_getinfo_request(uint8_t node_id, + const uavcan_getinfo_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id) +{ uavcan_frame_id_t frame_id; /* Set up the message ID */ @@ -276,14 +673,32 @@ void uavcan_tx_getinfo_request( (const uint8_t*)request, UAVCAN_GETINFO_CRC, MBAll); } - -can_error_t uavcan_rx_getinfo_response( - uint8_t node_id, - uavcan_getinfo_response_t *response, - uint8_t dest_node_id, - uint8_t transfer_id, - uint32_t timeout_ticks -) { +/**************************************************************************** + * Name: uavcan_rx_getinfo_response + * + * Description: + * This function attempts to receive a uavcan getinfo response message. + * + * Input Parameters: + * node_id - This node's node id + * response - A pointer a uavcan_getinfo_response_t to receive the + * response data into. + * dest_node_id - The remote node id to expect the message from. + * transfer_id - The expected transfer_id used to correlate this response + * message to the transmitted request message. + * timeout_ms - The number of milliseconds to wait for a response + * + * + * Returned value: + * CAN_OK on Success and CAN_ERROR otherwise. + * + ****************************************************************************/ +can_error_t uavcan_rx_getinfo_response(uint8_t node_id, + uavcan_getinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ms) +{ uavcan_frame_id_t frame_id; size_t length; can_error_t status; @@ -296,7 +711,7 @@ can_error_t uavcan_rx_getinfo_response( status = uavcan_rx_multiframe_(node_id, &frame_id, &length, (uint8_t*)response, UAVCAN_GETINFO_CRC, - timeout_ticks); + timeout_ms); if (status == CAN_OK && length == sizeof(uavcan_getinfo_response_t)) { return CAN_OK; @@ -306,153 +721,3 @@ can_error_t uavcan_rx_getinfo_response( } -static void uavcan_tx_multiframe_( - uavcan_frame_id_t *frame_id, - uint8_t dest_node_id, - size_t message_length, - const uint8_t *message, - uint16_t initial_crc, - uint8_t mailbox -) { - uint32_t i, m; - uint16_t frame_crc; - uint8_t payload[8]; - - /* Calculate the message CRC */ - frame_crc = crc16_signature(initial_crc, message_length, message); - - /* Ensure message ID has the frame details zeroed */ - frame_id->last_frame = 0u; - frame_id->frame_index = 0u; - - /* - Send the message -- only prepend CRC if the message will not fit within a - single frame - */ - payload[0] = dest_node_id; - m = 1u; - if (message_length > 7u) { - payload[m++] = (uint8_t)frame_crc; - payload[m++] = (uint8_t)(frame_crc >> 8u); - } - for (i = 0u; i < message_length; i++) { - payload[m++] = message[i]; - if (i == message_length - 1u) { - break; - } else if (m == 8u) { - can_tx(uavcan_make_message_id(frame_id), 8u, payload, mailbox); - frame_id->frame_index++; - payload[0] = dest_node_id; - m = 1u; - } - } - - /* Send the last (only?) frame */ - frame_id->last_frame = 1u; - can_tx(uavcan_make_message_id(frame_id), m, payload, mailbox); -} - - -static can_error_t uavcan_rx_multiframe_(uint8_t node_id, - uavcan_frame_id_t *frame_id, - size_t *message_length, - uint8_t *message, - uint16_t initial_crc, - uint32_t timeout_ticks) -{ - uavcan_frame_id_t rx_id; - size_t rx_length; - size_t m; - size_t i; - uint32_t rx_message_id; - uint16_t calculated_crc; - uint16_t message_crc; - uint8_t payload[8]; - uint8_t got_frame; - uint8_t num_frames; - - bl_timer_id timer = timer_allocate(modeTimeout|modeStarted, timeout_ticks, 0); - - num_frames = 0u; - rx_id.last_frame = 0u; - message_crc = 0u; - i = 0; - - do { - rx_message_id = 0u; - rx_length = 0u; - got_frame = can_rx(&rx_message_id, &rx_length, payload, MBAll); - if (!got_frame) { - continue; - } - - uavcan_parse_message_id(&rx_id, rx_message_id, frame_id->transfer_type); - - /* - Skip this frame if the source node ID is wrong, or if the data type or - transfer type do not match what we're expecting - */ - if ((frame_id->source_node_id != 0xFFu && - rx_id.source_node_id != frame_id->source_node_id) || - rx_id.transfer_type != frame_id->transfer_type || - rx_id.data_type_id != frame_id->data_type_id || - payload[0] != node_id) { - continue; - } - - /* - Get the CRC if this is the first frame of a multi-frame transfer. - - Also increase the timeout to UAVCAN_SERVICE_TIMEOUT_TICKS. - */ - if (rx_id.frame_index == 0u) { - num_frames = 0u; - frame_id->transfer_id = rx_id.transfer_id; - if (frame_id->source_node_id == 0xFFu) { - frame_id->source_node_id = rx_id.source_node_id; - } - } - - /* Skip if the transfer ID is wrong */ - if ((frame_id->transfer_id ^ rx_id.transfer_id) & 0x7u) { - continue; - } - - /* - Get the CRC and increase the service timeout if this is the first - frame - */ - if (num_frames == 0u && !rx_id.last_frame) { - timer_restart(timer, UAVCAN_SERVICE_TIMEOUT_MS); - message_crc = (uint16_t)(payload[1] | (payload[2] << 8u)); - m = 3u; - } else { - m = 1u; - } - - /* Copy message bytes to the response */ - for (; m < rx_length && i < *message_length; m++, i++) { - message[i] = payload[m]; - } - num_frames++; - - if (rx_id.last_frame) { - break; - } - } while (!timer_expired(timer)); - timer_free(timer); - if (!rx_id.last_frame) { - return CAN_ERROR; - } else { - /* Validate CRC */ - calculated_crc = crc16_signature(initial_crc, i, message); - - *message_length = i; - - if (num_frames == 1u || message_crc == calculated_crc) { - return CAN_OK; - } else { - return CAN_ERROR; - } - } -} diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h index 3b49ffce5..a60c2b4d0 100644 --- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h +++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h @@ -1,4 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Ben Dyer <ben_dyer@mac.com> + * Pavel Kirienko <pavel.kirienko@zubax.com> + * David Sidrane <david_s5@nscdg.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + #pragma once + /**************************************************************************** * Included Files ****************************************************************************/ @@ -8,10 +45,18 @@ #include <stdint.h> #include <stdlib.h> +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + #define UAVCAN_SERVICE_RETRIES 3 #define UAVCAN_SERVICE_TIMEOUT_MS 1000 #define UAVCAN_NODESTATUS_INTERVAL_MS 500 +/**************************************************************************** + * Public Type Definitions + ****************************************************************************/ + typedef enum { CAN_OK = 0, @@ -34,7 +79,7 @@ typedef struct packed_struct uavcan_frame_id_t { uint8_t source_node_id; uavcan_transfertype_t transfer_type; uint16_t data_type_id; - } uavcan_frame_id_t; +} uavcan_frame_id_t; typedef struct packed_struct uavcan_nodestatus_t { uint32_t uptime_sec; @@ -43,8 +88,6 @@ typedef struct packed_struct uavcan_nodestatus_t { } uavcan_nodestatus_t; #define UAVCAN_NODESTATUS_DTID 550u -#define UAVCAN_NODESTATUS_PUBLICATION_CYCLES (72000000u / 2u) -#define UAVCAN_NODESTATUS_STATUS_OK 0u #define UAVCAN_NODESTATUS_STATUS_INITIALIZING 1u #define UAVCAN_NODESTATUS_STATUS_WARNING 2u #define UAVCAN_NODESTATUS_STATUS_CRITICAL 3u @@ -175,64 +218,284 @@ typedef struct packed_struct uavcan_read_response_t { #define UAVCAN_ALLOCATION_CRC 0x7BAAu +/**************************************************************************** + * Global Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ +/**************************************************************************** + * Name: uavcan_pack_nodestatus + * + * Description: + * This function formats the data of a uavcan_nodestatus_t structure into + * an array of bytes. + * + * Input Parameters: + * data - The array of bytes to populate. + * message - The uavcan_nodestatus_t to pack into the data + * + * Returned value: + * Number of bytes written. + * + ****************************************************************************/ + +size_t uavcan_pack_nodestatus(uint8_t *data, + const uavcan_nodestatus_t *message); + +/**************************************************************************** + * Name: uavcan_pack_logmessage + * + * Description: + * This function formats the data of a uavcan_logmessage_t structure into + * an array of bytes. + * + * Input Parameters: + * data - The array of bytes to populate. + * message - The uavcan_logmessage_t to pack into the data + * + * Returned value: + * Number of bytes written. + * + ****************************************************************************/ + +size_t uavcan_pack_logmessage(uint8_t *data, + const uavcan_logmessage_t *message); + +/**************************************************************************** + * Name: uavcan_make_message_id + * + * Description: + * This function formats the data of a uavcan_frame_id_t structure into + * a unit32. + * + * Input Parameters: + * frame_id - The uavcan_frame_id_t to pack. + * + * Returned value: + * A unit32 that is the message id formed from packing the + * uavcan_frame_id_t. + * + ****************************************************************************/ -size_t uavcan_pack_nodestatus( - uint8_t *data, - const uavcan_nodestatus_t *payload -); -size_t uavcan_pack_logmessage( - uint8_t *data, - const uavcan_logmessage_t *payload -); uint32_t uavcan_make_message_id(const uavcan_frame_id_t *frame_id); + +/**************************************************************************** + * Name: uavcan_parse_message_id + * + * Description: + * This function formats the data of a uavcan message_id contained in a uint32 + * into uavcan_frame_id_t structure. + * + * Input Parameters: + * frame_id - A pointer to a uavcan_frame_id_t parse the message_id into. + * message_id - The message id to parse into the uavcan_frame_id_t + * expected_id -The expected uavcan data_type_id that has been parsed into + * frame_id's data_type_id field + * + * Returned value: + * The result of comparing the data_type_id to the expected_id + * Non Zero if they match otherwise zero. + * + ****************************************************************************/ int uavcan_parse_message_id(uavcan_frame_id_t *frame_id, uint32_t message_id, - uint16_t expected_id); -void uavcan_tx_nodestatus( - uint8_t node_id, - uint32_t uptime_sec, - uint8_t status_code -); -void uavcan_tx_allocation_message( - uint8_t requested_node_id, - size_t unique_id_length, - const uint8_t *unique_id, - uint8_t unique_id_offset, - uint8_t transfer_id -); -void uavcan_tx_getnodeinfo_response( - uint8_t node_id, - uavcan_getnodeinfo_response_t *response, - uint8_t dest_node_id, - uint8_t transfer_id -); -can_error_t uavcan_rx_beginfirmwareupdate_request( - uint8_t node_id, - uavcan_beginfirmwareupdate_request_t *request, - uavcan_frame_id_t *out_frame_id -); -void uavcan_tx_read_request( - uint8_t node_id, - const uavcan_read_request_t *request, - uint8_t dest_node_id, - uint8_t transfer_id -); -can_error_t uavcan_rx_read_response( - uint8_t node_id, - uavcan_read_response_t *response, - uint8_t dest_node_id, - uint8_t transfer_id, - uint32_t timeout_ticks -); -void uavcan_tx_getinfo_request( - uint8_t node_id, - const uavcan_getinfo_request_t *request, - uint8_t dest_node_id, - uint8_t transfer_id -); -can_error_t uavcan_rx_getinfo_response( - uint8_t node_id, - uavcan_getinfo_response_t *response, - uint8_t dest_node_id, - uint8_t transfer_id, - uint32_t timeout_ticks -); + uint16_t expected_id); + +/**************************************************************************** + * Name: uavcan_tx_nodestatus + * + * Description: + * This function sends a uavcan nodestatus message + * + * Input Parameters: + * node_id - This node's node id + * uptime_sec - This node's uptime in seconds. + * status_code - This node's current status code + * + * Returned value: + * None. + * + ****************************************************************************/ + +void uavcan_tx_nodestatus(uint8_t node_id, uint32_t uptime_sec, + uint8_t status_code); + +/**************************************************************************** + * Name: uavcan_tx_allocation_message + * + * Description: + * This function sends a uavcan allocation message. + * + * Input Parameters: + * requested_node_id - This node's preferred node id 0 for no preference. + * unique_id_length - This node's length of it's unique identifier. + * unique_id - A pointer to the bytes that represent unique + * identifier. + * unique_id_offset - The offset equal 0 or the number of bytes in the + * the last received message that matched the unique ID + * field. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message + * + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_allocation_message(uint8_t requested_node_id, + size_t unique_id_length, + const uint8_t *unique_id, + uint8_t unique_id_offset, + uint8_t transfer_id); + +/**************************************************************************** + * Name: uavcan_tx_getnodeinfo_response + * + * Description: + * This function sends a uavcan getnodeinfo response to a getnodeinfo + * request message. + * + * Input Parameters: + * node_id - This node's node id + * response - A pointer to this node's uavcan_getnodeinfo_response_t + * response data. + * dest_node_id - The destination node id to send the message to. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_getnodeinfo_response(uint8_t node_id, + uavcan_getnodeinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id); + +/**************************************************************************** + * Name: uavcan_rx_beginfirmwareupdate_request + * + * Description: + * This function attempts to receive a uavcan beginfirmwareupdate request + * message + * + * Input Parameters: + * node_id - This node's node id + * request - A pointer a uavcan_beginfirmwareupdate_request_t to + * receive the request data into. + * frame_id - A pointer to a uavcan_frame_id_t to return frame_id + * components in. + * + * + * Returned value: + * CAN_OK on Success and CAN_ERROR otherwise. + * + ****************************************************************************/ + +can_error_t uavcan_rx_beginfirmwareupdate_request(uint8_t node_id, + uavcan_beginfirmwareupdate_request_t *request, + uavcan_frame_id_t *frame_id); + +/**************************************************************************** + * Name: uavcan_tx_read_request + * + * Description: + * This function sends a uavcan read request message. + * + * Input Parameters: + * node_id - This node's node id + * request - A pointer a uavcan_read_request_t to + * send. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_read_request(uint8_t node_id, + const uavcan_read_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id); + +/**************************************************************************** + * Name: uavcan_rx_read_response + * + * Description: + * This function attempts to receive a uavcan read response message. + * + * Input Parameters: + * node_id - This node's node id + * response - A pointer a uavcan_read_response_t to receive the + * response data into. + * dest_node_id - The remote node id to expect the message from. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * timeout_ms - The number of milliseconds to wait for a response + * + * + * Returned value: + * CAN_OK on Success and CAN_ERROR otherwise. + * + ****************************************************************************/ + +can_error_t uavcan_rx_read_response(uint8_t node_id, + uavcan_read_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ms); + +/**************************************************************************** + * Name: uavcan_tx_getinfo_request + * + * Description: + * This function sends a uavcan getinfo request message. + * + * Input Parameters: + * node_id - This node's node id + * request - A pointer a uavcan_getinfo_request_t to + * send. + * dest_node_id - The destination node id to send the message to. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * + * Returned value: + * None + * + ****************************************************************************/ + +void uavcan_tx_getinfo_request(uint8_t node_id, + const uavcan_getinfo_request_t *request, + uint8_t dest_node_id, + uint8_t transfer_id); + +/**************************************************************************** + * Name: uavcan_rx_getinfo_response + * + * Description: + * This function attempts to receive a uavcan getinfo response message. + * + * Input Parameters: + * node_id - This node's node id + * response - A pointer a uavcan_getinfo_response_t to receive the + * response data into. + * dest_node_id - The remote node id to expect the message from. + * transfer_id - An incrementing count used to correlate a received + * message to this transmitted message. + * timeout_ms - The number of milliseconds to wait for a response + * + * + * Returned value: + * CAN_OK on Success and CAN_ERROR otherwise. + * + ****************************************************************************/ +can_error_t uavcan_rx_getinfo_response(uint8_t node_id, + uavcan_getinfo_response_t *response, + uint8_t dest_node_id, + uint8_t transfer_id, + uint32_t timeout_ms); |