aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorDavid Sidrane <david_s5@nscdg.com>2015-04-21 07:59:31 -1000
committerDavid Sidrane <david_s5@nscdg.com>2015-04-22 02:30:14 -1000
commit9fb0031b56993a6f42c3eb5b325f22ea80ac9290 (patch)
treea4783b3160aaa7405c6dbf8e61cd0cfd9275f407
parentf2411bb6d29bd4b8c0feb70ed4bdb0617ac01d14 (diff)
downloadpx4-firmware-9fb0031b56993a6f42c3eb5b325f22ea80ac9290.tar.gz
px4-firmware-9fb0031b56993a6f42c3eb5b325f22ea80ac9290.tar.bz2
px4-firmware-9fb0031b56993a6f42c3eb5b325f22ea80ac9290.zip
Stucture packing and Docs
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c203
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h260
-rw-r--r--src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h425
3 files changed, 592 insertions, 296 deletions
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c
index b8d649ea6..d9e7ca5e9 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.c
+++ b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.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 <stdint.h>
@@ -10,15 +50,21 @@
#include "boot_app_shared.h"
#include "crc.h"
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
#define BOOTLOADER_COMMON_APP_SIGNATURE 0xB0A04150u
#define BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE 0xB0A0424Cu
+
/* CAN_FiRx where (i=0..27|13, x=1, 2)
* STM32_CAN1_FIR(i,x)
* Using i = 2 does not requier there block
* to be enabled nor FINIT in CAN_FMR to be set.
* todo:Validate this claim on F2, F3
*/
+
#define crc_HiLOC STM32_CAN1_FIR(2,1)
#define crc_LoLOC STM32_CAN1_FIR(2,2)
#define signature_LOC STM32_CAN1_FIR(3,1)
@@ -27,31 +73,63 @@
#define CRC_H 1
#define CRC_L 0
-inline static void read(bootloader_app_shared_t * pcommon)
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: read
+ ****************************************************************************/
+
+inline static void read(bootloader_app_shared_t * pshared)
{
- pcommon->signature = getreg32(signature_LOC);
- pcommon->bus_speed = getreg32(bus_speed_LOC);
- pcommon->node_id = getreg32(node_id_LOC);
- pcommon->crc.ul[CRC_L] = getreg32(crc_LoLOC);
- pcommon->crc.ul[CRC_H] = getreg32(crc_HiLOC);
+ pshared->signature = getreg32(signature_LOC);
+ pshared->bus_speed = getreg32(bus_speed_LOC);
+ pshared->node_id = getreg32(node_id_LOC);
+ pshared->crc.ul[CRC_L] = getreg32(crc_LoLOC);
+ pshared->crc.ul[CRC_H] = getreg32(crc_HiLOC);
}
-inline static void write(bootloader_app_shared_t * pcommon)
+/****************************************************************************
+ * Name: write
+ ****************************************************************************/
+
+inline static void write(bootloader_app_shared_t * pshared)
{
- putreg32(pcommon->signature, signature_LOC);
- putreg32(pcommon->bus_speed, bus_speed_LOC);
- putreg32(pcommon->node_id, node_id_LOC);
- putreg32(pcommon->crc.ul[CRC_L], crc_LoLOC);
- putreg32(pcommon->crc.ul[CRC_H], crc_HiLOC);
+ putreg32(pshared->signature, signature_LOC);
+ putreg32(pshared->bus_speed, bus_speed_LOC);
+ putreg32(pshared->node_id, node_id_LOC);
+ putreg32(pshared->crc.ul[CRC_L], crc_LoLOC);
+ putreg32(pshared->crc.ul[CRC_H], crc_HiLOC);
}
-static uint64_t calulate_signature(bootloader_app_shared_t * pcommon)
+/****************************************************************************
+ * Name: calulate_signature
+ ****************************************************************************/
+
+static uint64_t calulate_signature(bootloader_app_shared_t * pshared)
{
- size_t size = sizeof(bootloader_app_shared_t) - sizeof(pcommon->crc);
+ size_t size = sizeof(bootloader_app_shared_t) - sizeof(pshared->crc);
uint64_t crc = CRC64_INITIAL;
- uint8_t *protected = (uint8_t *) & pcommon->signature;
+ uint8_t *protected = (uint8_t *) & pshared->signature;
while (size--)
{
@@ -61,12 +139,16 @@ static uint64_t calulate_signature(bootloader_app_shared_t * pcommon)
return crc;
}
-void bootloader_app_shared_init(bootloader_app_shared_t * pcommon, eRole_t role)
+
+/****************************************************************************
+ * Name: bootloader_app_shared_init
+ ****************************************************************************/
+static void bootloader_app_shared_init(bootloader_app_shared_t * pshared, eRole_t role)
{
- memset(pcommon, 0, sizeof(bootloader_app_shared_t));
+ memset(pshared, 0, sizeof(bootloader_app_shared_t));
if (role != Invalid)
{
- pcommon->signature =
+ pshared->signature =
(role ==
App ? BOOTLOADER_COMMON_APP_SIGNATURE :
BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE);
@@ -74,8 +156,39 @@ void bootloader_app_shared_init(bootloader_app_shared_t * pcommon, eRole_t role)
}
-int bootloader_app_shared_read(bootloader_app_shared_t * pcommon,
- eRole_t role)
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+/****************************************************************************
+ * Name: bootloader_app_shared_read
+ *
+ * Description:
+ * Based on the role requested, this function will conditionally populate
+ * a bootloader_app_shared_t structure from the physical locations used
+ * to transfer the shared data to/from an application (internal data) .
+ *
+ * The functions will only populate the structure and return a status
+ * indicating success, if the internal data has the correct signature as
+ * requested by the Role AND has a valid crc.
+ *
+ * Input Parameters:
+ * shared - A pointer to a bootloader_app_shared_t return the data in if
+ * the internal data is valid for the requested Role
+ * role - An eRole_t of App or BootLoader to validate the internal data
+ * against. For a Bootloader this would be the value of App to
+ * read the application passed data.
+ *
+ * Returned value:
+ * OK - Indicates that the internal data has been copied to callers
+ * bootloader_app_shared_t structure.
+ *
+ * -EBADR - The Role or crc of the internal data was not valid. The copy
+ * did not occur.
+ *
+ ****************************************************************************/
+
+int bootloader_app_shared_read(bootloader_app_shared_t * shared,
+ eRole_t role)
{
int rv = -EBADR;
bootloader_app_shared_t working;
@@ -86,16 +199,40 @@ int bootloader_app_shared_read(bootloader_app_shared_t * pcommon,
: working.signature == BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE)
&& (working.crc.ull == calulate_signature(&working)))
{
- *pcommon = working;
+ *shared = working;
rv = OK;
}
return rv;
}
-void bootloader_app_shared_write(bootloader_app_shared_t * common,
- eRole_t role)
+/****************************************************************************
+ * Name: bootloader_app_shared_write
+ *
+ * Description:
+ * Based on the role, this function will commit the data passed
+ * into the physical locations used to transfer the shared data to/from
+ * an application (internal data) .
+ *
+ * The functions will populate the signature and crc the data
+ * based on the provided Role.
+ *
+ * Input Parameters:
+ * shared - A pointer to a bootloader_app_shared_t data to commit to
+ * the internal data for passing to/from an application.
+ * role - An eRole_t of App or BootLoader to use in the internal data
+ * to be passed to/from an application. For a Bootloader this
+ * would be the value of Bootloader to write to the passed data.
+ * to the application via the internal data.
+ *
+ * Returned value:
+ * None.
+ *
+ ****************************************************************************/
+
+void bootloader_app_shared_write(bootloader_app_shared_t * shared,
+ eRole_t role)
{
- bootloader_app_shared_t working = *common;
+ bootloader_app_shared_t working = *shared;
working.signature =
(role ==
App ? BOOTLOADER_COMMON_APP_SIGNATURE :
@@ -105,6 +242,24 @@ void bootloader_app_shared_write(bootloader_app_shared_t * common,
}
+/****************************************************************************
+ * Name: bootloader_app_shared_invalidate
+ *
+ * Description:
+ * Invalidates the data passed the physical locations used to transfer
+ * the shared data to/from an application (internal data) .
+ *
+ * The functions will invalidate the signature and crc and shoulf be used
+ * to prevent deja vu.
+ *
+ * Input Parameters:
+ * None.
+ *
+ * Returned value:
+ * None.
+ *
+ ****************************************************************************/
+
void bootloader_app_shared_invalidate(void)
{
bootloader_app_shared_t working;
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h
index b13ba1131..12f683f58 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h
+++ b/src/drivers/boards/px4cannode-v1/bootloader/common/boot_app_shared.h
@@ -1,66 +1,97 @@
-
-/*
+/****************************************************************************
*
- */
+ * 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
-/*
-Application firmware descriptor.
-
-This is located by the linker script somewhere aftert the vector table.
-(within the first several kilobytes of the application);
-This struct must be aligned on an 8-byte boundary.
-
-The bootloader scans through the application FLASH image until it
-finds the signature.
-
-The image_crc is calculated as follows:
- 1) All fields of this struct must be initalized with the correct information about
- the firmware image bin file (Here after refered to as image)
- 2) image_crc set to 0;
- 3) The CRC 64 is caculated over the image from offset 0 up to and including the
- last byte of the image file.
- 4) The caculated CRC 64 is stored in image_crc
- 5) The new image file is then written to a file a ".img" extention.
-*/
-typedef struct
- {
- uint64_t signature;
- uint64_t image_crc;
- uint32_t image_size;
- uint32_t vcs_commit;
- uint8_t major_version;
- uint8_t minor_version;
- uint8_t reserved[6];
- }
-__attribute__ ((packed)) app_descriptor_t;
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+# include <nuttx/compiler.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Define the signature for the Application descriptor as 'APDesc' and a
+ * revision number of 00 used in app_descriptor_t
+ */
-// APDesc00
#define APP_DESCRIPTOR_SIGNATURE_ID 'A','P','D','e','s','c'
#define APP_DESCRIPTOR_SIGNATURE_REV '0','0'
#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV
-/*
-Bootloader/app common structure.
-
-The data in this struct is passed in SRAM or the the CAN filter registers
-from bootloader to application and application to bootloader.
+/****************************************************************************
+ * Public Type Definitions
+ ****************************************************************************/
-Do not assume any mapping or location for the passing of this data
-that is done in the read and write routines and is abstracted on purpose.
+/* eRole defines the role of the bootloader_app_shared_t structure */
-The application must write BOOTLOADER_COMMON_APP_SIGNATURE to the signature
-field when passing data to the bootloader; when the bootloader passes data
-to the app, it must write BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE to the
-signature field.
+typedef enum eRole {
+ Invalid,
+ App,
+ BootLoader
+} eRole_t;
-The CRC is calculated over the structure from signature to the last byte.
-The resuluting values are then copied to the CAN filter registers by
-bootloader_app_shared_read and bootloader_app_shared_write.
+/****************************************************************************
+ *
+ * Bootloader and Application shared structure.
+ *
+ * The data in this structure is passed in SRAM or the the CAN filter
+ * registers from bootloader to application and application to bootloader.
+ *
+ * Do not assume any mapping or location for the passing of this data
+ * that is done in the read and write routines and is abstracted by design.
+ *
+ * For reference, the following is performed based on eRole in API calls
+ * defined below:
+ *
+ * The application must write BOOTLOADER_COMMON_APP_SIGNATURE to the
+ * signature field when passing data to the bootloader; when the
+ * bootloader passes data to the application, it must write
+ * BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE to the signature field.
+ *
+ * The CRC is calculated over the structure from signature to the
+ * last byte. The resulting values are then copied to the CAN filter
+ * registers by bootloader_app_shared_read and
+ * bootloader_app_shared_write.
+ *
+****************************************************************************/
-*/
-typedef struct
- {
+typedef struct packed_struct bootloader_app_shared_t {
union
{
uint64_t ull;
@@ -69,20 +100,125 @@ typedef struct
uint32_t signature;
uint32_t bus_speed;
uint32_t node_id;
- } __attribute__ ((packed)) bootloader_app_shared_t;
+} bootloader_app_shared_t ;
+
+/****************************************************************************
+ *
+ * Application firmware descriptor.
+ *
+ * This structure located by the linker script somewhere after the vector table.
+ * (within the first several kilobytes of the beginning address of the
+ * application);
+ *
+ * This structure must be aligned on an 8-byte boundary.
+ *
+ * The bootloader will scan through the application FLASH image until it
+ * finds the signature.
+ *
+ * The image_crc is calculated as follows:
+ * 1) All fields of this structure must be initialized with the correct
+ * information about the firmware image bin file
+ * (Here after refereed to as image)
+ * 2) image_crc set to 0;
+ * 3) The CRC 64 is calculated over the image from offset 0 up to and including the
+ * last byte of the image file.
+ * 4) The calculated CRC 64 is stored in image_crc
+ * 5) The new image file is then written to a file a ".img" extension.
+ *
+****************************************************************************/
-typedef enum eRole
+typedef struct packed_struct app_descriptor_t
{
- Invalid,
- App,
- BootLoader
- } eRole_t;
+ uint64_t signature;
+ uint64_t image_crc;
+ uint32_t image_size;
+ uint32_t vcs_commit;
+ uint8_t major_version;
+ uint8_t minor_version;
+ uint8_t reserved[6];
+} app_descriptor_t;
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
-void bootloader_app_shared_init(bootloader_app_shared_t * common,
- eRole_t role);
-int bootloader_app_shared_read(bootloader_app_shared_t * common,
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+/****************************************************************************
+ * Name: bootloader_app_shared_read
+ *
+ * Description:
+ * Based on the role requested, this function will conditionally populate
+ * a bootloader_app_shared_t structure from the physical locations used
+ * to transfer the shared data to/from an application (internal data) .
+ *
+ * The functions will only populate the structure and return a status
+ * indicating success, if the internal data has the correct signature as
+ * requested by the Role AND has a valid crc.
+ *
+ * Input Parameters:
+ * shared - A pointer to a bootloader_app_shared_t return the data in if
+ * the internal data is valid for the requested Role
+ * role - An eRole_t of App or BootLoader to validate the internal data
+ * against. For a Bootloader this would be the value of App to
+ * read the application passed data.
+ *
+ * Returned value:
+ * OK - Indicates that the internal data has been copied to callers
+ * bootloader_app_shared_t structure.
+ *
+ * -EBADR - The Role or crc of the internal data was not valid. The copy
+ * did not occur.
+ *
+ ****************************************************************************/
+
+int bootloader_app_shared_read(bootloader_app_shared_t * shared,
eRole_t role);
-void bootloader_app_shared_write(bootloader_app_shared_t * common,
- eRole_t role);
+
+/****************************************************************************
+ * Name: bootloader_app_shared_write
+ *
+ * Description:
+ * Based on the role, this function will commit the data passed
+ * into the physical locations used to transfer the shared data to/from
+ * an application (internal data) .
+ *
+ * The functions will populate the signature and crc the data
+ * based on the provided Role.
+ *
+ * Input Parameters:
+ * shared - A pointer to a bootloader_app_shared_t data to commit to
+ * the internal data for passing to/from an application.
+ * role - An eRole_t of App or BootLoader to use in the internal data
+ * to be passed to/from an application. For a Bootloader this
+ * would be the value of Bootloader to write to the passed data.
+ * to the application via the internal data.
+ *
+ * Returned value:
+ * None.
+ *
+ ****************************************************************************/
+
+void bootloader_app_shared_write(bootloader_app_shared_t * shared,
+ eRole_t role);
+
+/****************************************************************************
+ * Name: bootloader_app_shared_invalidate
+ *
+ * Description:
+ * Invalidates the data passed the physical locations used to transfer
+ * the shared data to/from an application (internal data) .
+ *
+ * The functions will invalidate the signature and crc and should be used
+ * to prevent deja vu.
+ *
+ * Input Parameters:
+ * None.
+ *
+ * Returned value:
+ * None.
+ *
+ ****************************************************************************/
+
void bootloader_app_shared_invalidate(void);
diff --git a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h
index f1530a692..faee7ae10 100644
--- a/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h
+++ b/src/drivers/boards/px4cannode-v1/bootloader/uavcan/protocol.h
@@ -1,4 +1,9 @@
#pragma once
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+# include <nuttx/compiler.h>
#include <stdint.h>
#include <stdlib.h>
@@ -15,219 +20,219 @@ typedef enum
} can_error_t;
/* UAVCAN message formats */
- typedef enum {
- SERVICE_RESPONSE = 0,
- SERVICE_REQUEST = 1,
- MESSAGE_BROADCAST = 2,
- MESSAGE_UNICAST = 3
- } uavcan_transfertype_t;
-
- typedef struct {
+typedef enum {
+ SERVICE_RESPONSE = 0,
+ SERVICE_REQUEST = 1,
+ MESSAGE_BROADCAST = 2,
+ MESSAGE_UNICAST = 3
+} uavcan_transfertype_t;
+
+typedef struct packed_struct uavcan_frame_id_t {
uint8_t transfer_id;
uint8_t last_frame;
uint8_t frame_index;
uint8_t source_node_id;
uavcan_transfertype_t transfer_type;
uint16_t data_type_id;
- } __attribute__((packed)) uavcan_frame_id_t;
-
- typedef struct {
- uint32_t uptime_sec;
- uint8_t status_code;
- uint16_t vendor_specific_status_code;
- } __attribute__((packed)) 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
-
-
- typedef struct {
- uint8_t major;
- uint8_t minor;
- uint8_t optional_field_mask;
- uint32_t vcs_commit;
- uint64_t image_crc;
- } __attribute__((packed)) uavcan_softwareversion_t;
-
- typedef struct {
- uint8_t major;
- uint8_t minor;
- uint8_t unique_id[16];
- uint8_t certificate_of_authenticity_length;
- uint8_t certificate_of_authenticity[255];
- } __attribute__((packed)) uavcan_hardwareversion_t;
-
- typedef struct {
- uint8_t nodestatus[6];
-
- uavcan_softwareversion_t software_version;
- uavcan_hardwareversion_t hardware_version;
-
- uint8_t name[80];
- uint8_t name_length;
- } __attribute__((packed)) uavcan_getnodeinfo_response_t;
-
- #define UAVCAN_GETNODEINFO_DTID 551u
- #define UAVCAN_GETNODEINFO_CRC 0x14BBu
-
-
- typedef struct {
- uint8_t node_id; /* bottom bit is the first part flag */
- uint8_t unique_id[16];
- } __attribute__((packed)) uavcan_allocation_t;
-
- #define UAVCAN_DYNAMICNODEIDALLOCATION_DTID 559u
-
-
- typedef struct {
- uint8_t level;
- uint8_t message[2];
- } __attribute__((packed)) uavcan_logmessage_t;
-
- #define UAVCAN_LOGMESSAGE_DTID 1023u
- #define UAVCAN_LOGMESSAGE_LEVEL_DEBUG 0u
- #define UAVCAN_LOGMESSAGE_LEVEL_INFO 1u
- #define UAVCAN_LOGMESSAGE_LEVEL_WARNING 2u
- #define UAVCAN_LOGMESSAGE_LEVEL_ERROR 3u
- #define UAVCAN_LOGMESSAGE_SOURCE_0 'B'
- #define UAVCAN_LOGMESSAGE_SOURCE_1 'O'
- #define UAVCAN_LOGMESSAGE_SOURCE_2 'O'
- #define UAVCAN_LOGMESSAGE_SOURCE_3 'T'
-
- #define UAVCAN_LOGMESSAGE_STAGE_INIT 'I'
- #define UAVCAN_LOGMESSAGE_STAGE_GET_INFO 'G'
- #define UAVCAN_LOGMESSAGE_STAGE_ERASE 'E'
- #define UAVCAN_LOGMESSAGE_STAGE_READ 'R'
- #define UAVCAN_LOGMESSAGE_STAGE_PROGRAM 'P'
- #define UAVCAN_LOGMESSAGE_STAGE_VALIDATE 'V'
- #define UAVCAN_LOGMESSAGE_STAGE_FINALIZE 'F'
- #define UAVCAN_LOGMESSAGE_RESULT_START 's'
- #define UAVCAN_LOGMESSAGE_RESULT_FAIL 'f'
- #define UAVCAN_LOGMESSAGE_RESULT_OK 'o'
-
-
- typedef struct {
- uint8_t source_node_id;
- uint8_t path[200];
- uint8_t path_length;
- } __attribute__((packed)) uavcan_beginfirmwareupdate_request_t;
-
- typedef struct {
- uint8_t error;
- } __attribute__((packed)) uavcan_beginfirmwareupdate_response_t;
-
- #define UAVCAN_BEGINFIRMWAREUPDATE_DTID 580u
- #define UAVCAN_BEGINFIRMWAREUPDATE_CRC 0x729Eu
- #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_OK 0u
- #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_INVALID_MODE 1u
- #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_IN_PROGRESS 2u
- #define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_UNKNOWN 255u
-
-
- typedef struct {
- uint8_t path[200];
- uint8_t path_length;
- } __attribute__((packed)) uavcan_getinfo_request_t;
-
- typedef struct {
- uint64_t crc64;
- uint32_t size;
- uint16_t error;
- uint8_t entry_type;
- } __attribute__((packed)) uavcan_getinfo_response_t;
-
- #define UAVCAN_GETINFO_DTID 585u
- #define UAVCAN_GETINFO_CRC 0x37A0u
- #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_FILE 0x01u
- #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_DIRECTORY 0x02u
- #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_SYMLINK 0x04u
- #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_READABLE 0x08u
- #define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_WRITEABLE 0x10u
-
-
- typedef struct {
- uint32_t offset;
- uint8_t path[200];
- uint8_t path_length;
- } __attribute__((packed)) uavcan_read_request_t;
-
- typedef struct {
- uint16_t error;
- uint8_t data[250];
- uint8_t data_length;
- } __attribute__((packed)) uavcan_read_response_t;
-
- #define UAVCAN_READ_DTID 588u
- #define UAVCAN_READ_CRC 0x2921u
-
- #define UAVCAN_FILE_ERROR_OK 0u
- /* Left the others out for now because we don't really care why it failed */
-
-
- #define UAVCAN_ALLOCATION_CRC 0x7BAAu
-
-
- 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);
- 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
- );
+ } uavcan_frame_id_t;
+
+typedef struct packed_struct uavcan_nodestatus_t {
+ uint32_t uptime_sec;
+ uint8_t status_code;
+ uint16_t vendor_specific_status_code;
+} 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
+
+
+typedef struct packed_struct uavcan_softwareversion_t {
+ uint8_t major;
+ uint8_t minor;
+ uint8_t optional_field_mask;
+ uint32_t vcs_commit;
+ uint64_t image_crc;
+} uavcan_softwareversion_t;
+
+typedef struct packed_struct uavcan_hardwareversion_t {
+ uint8_t major;
+ uint8_t minor;
+ uint8_t unique_id[16];
+ uint8_t certificate_of_authenticity_length;
+ uint8_t certificate_of_authenticity[255];
+} uavcan_hardwareversion_t;
+
+typedef struct packed_struct uavcan_getnodeinfo_response_t {
+ uint8_t nodestatus[6];
+
+ uavcan_softwareversion_t software_version;
+ uavcan_hardwareversion_t hardware_version;
+
+ uint8_t name[80];
+ uint8_t name_length;
+} uavcan_getnodeinfo_response_t;
+
+#define UAVCAN_GETNODEINFO_DTID 551u
+#define UAVCAN_GETNODEINFO_CRC 0x14BBu
+
+
+typedef struct packed_struct uavcan_allocation_t {
+ uint8_t node_id; /* bottom bit is the first part flag */
+ uint8_t unique_id[16];
+} uavcan_allocation_t;
+
+#define UAVCAN_DYNAMICNODEIDALLOCATION_DTID 559u
+
+
+typedef struct packed_struct uavcan_logmessage_t {
+ uint8_t level;
+ uint8_t message[2];
+} uavcan_logmessage_t;
+
+#define UAVCAN_LOGMESSAGE_DTID 1023u
+#define UAVCAN_LOGMESSAGE_LEVEL_DEBUG 0u
+#define UAVCAN_LOGMESSAGE_LEVEL_INFO 1u
+#define UAVCAN_LOGMESSAGE_LEVEL_WARNING 2u
+#define UAVCAN_LOGMESSAGE_LEVEL_ERROR 3u
+#define UAVCAN_LOGMESSAGE_SOURCE_0 'B'
+#define UAVCAN_LOGMESSAGE_SOURCE_1 'O'
+#define UAVCAN_LOGMESSAGE_SOURCE_2 'O'
+#define UAVCAN_LOGMESSAGE_SOURCE_3 'T'
+
+#define UAVCAN_LOGMESSAGE_STAGE_INIT 'I'
+#define UAVCAN_LOGMESSAGE_STAGE_GET_INFO 'G'
+#define UAVCAN_LOGMESSAGE_STAGE_ERASE 'E'
+#define UAVCAN_LOGMESSAGE_STAGE_READ 'R'
+#define UAVCAN_LOGMESSAGE_STAGE_PROGRAM 'P'
+#define UAVCAN_LOGMESSAGE_STAGE_VALIDATE 'V'
+#define UAVCAN_LOGMESSAGE_STAGE_FINALIZE 'F'
+#define UAVCAN_LOGMESSAGE_RESULT_START 's'
+#define UAVCAN_LOGMESSAGE_RESULT_FAIL 'f'
+#define UAVCAN_LOGMESSAGE_RESULT_OK 'o'
+
+
+typedef struct packed_struct uavcan_beginfirmwareupdate_request_t {
+ uint8_t source_node_id;
+ uint8_t path[200];
+ uint8_t path_length;
+} uavcan_beginfirmwareupdate_request_t;
+
+typedef struct packed_struct uavcan_beginfirmwareupdate_response_t {
+ uint8_t error;
+} uavcan_beginfirmwareupdate_response_t;
+
+#define UAVCAN_BEGINFIRMWAREUPDATE_DTID 580u
+#define UAVCAN_BEGINFIRMWAREUPDATE_CRC 0x729Eu
+#define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_OK 0u
+#define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_INVALID_MODE 1u
+#define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_IN_PROGRESS 2u
+#define UAVCAN_BEGINFIRMWAREUPDATE_ERROR_UNKNOWN 255u
+
+
+typedef struct packed_struct uavcan_getinfo_request_t {
+ uint8_t path[200];
+ uint8_t path_length;
+} uavcan_getinfo_request_t;
+
+typedef struct packed_struct uavcan_getinfo_response_t {
+ uint64_t crc64;
+ uint32_t size;
+ uint16_t error;
+ uint8_t entry_type;
+} uavcan_getinfo_response_t;
+
+#define UAVCAN_GETINFO_DTID 585u
+#define UAVCAN_GETINFO_CRC 0x37A0u
+#define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_FILE 0x01u
+#define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_DIRECTORY 0x02u
+#define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_SYMLINK 0x04u
+#define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_READABLE 0x08u
+#define UAVCAN_GETINFO_ENTRY_TYPE_FLAG_WRITEABLE 0x10u
+
+
+typedef struct packed_struct uavcan_read_request_t {
+ uint32_t offset;
+ uint8_t path[200];
+ uint8_t path_length;
+} uavcan_read_request_t;
+
+typedef struct packed_struct uavcan_read_response_t {
+ uint16_t error;
+ uint8_t data[250];
+ uint8_t data_length;
+} uavcan_read_response_t;
+
+#define UAVCAN_READ_DTID 588u
+#define UAVCAN_READ_CRC 0x2921u
+
+#define UAVCAN_FILE_ERROR_OK 0u
+/* Left the others out for now because we don't really care why it failed */
+
+
+#define UAVCAN_ALLOCATION_CRC 0x7BAAu
+
+
+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);
+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
+);