diff --git a/.gitignore b/.gitignore index 6f2af989..66cc22cb 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,5 @@ build .project .cproject .settings/language.settings.xml +.DS_Store +workspace.code-workspace \ No newline at end of file diff --git a/hwconf/hw_410.h b/hwconf/hw_410.h index b9c4bfa3..e0d0ca1b 100644 --- a/hwconf/hw_410.h +++ b/hwconf/hw_410.h @@ -19,6 +19,8 @@ #define HW_410_H_ #define HW_NAME "410" +#define HW_MAJOR 4 +#define HW_MINOR 10 // Macros #define ENABLE_GATE() palSetPad(GPIOC, 10) diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h index b7b837dc..4a7fd59c 100644 --- a/hwconf/hw_60.h +++ b/hwconf/hw_60.h @@ -30,6 +30,9 @@ #define HW_NAME "60" #endif +#define HW_MAJOR 6 +#define HW_MINOR 0 + // HW properties #define HW_HAS_DRV8301 #define HW_HAS_3_SHUNTS diff --git a/libcanard/canard.mk b/libcanard/canard.mk index a8c8a8c4..cbb33e46 100644 --- a/libcanard/canard.mk +++ b/libcanard/canard.mk @@ -2,7 +2,18 @@ CANARDSRC = libcanard/canard.c \ libcanard/canard_driver.c \ libcanard/dsdl/uavcan/equipment/esc/esc_Status.c \ libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c \ - libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c + libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c \ + libcanard/dsdl/uavcan/protocol/protocol_GetNodeInfo.c \ + libcanard/dsdl/uavcan/protocol/protocol_HardwareVersion.c \ + libcanard/dsdl/uavcan/protocol/protocol_NodeStatus.c \ + libcanard/dsdl/uavcan/protocol/protocol_SoftwareVersion.c \ + libcanard/dsdl/uavcan/protocol/param/param_Empty.c \ + libcanard/dsdl/uavcan/protocol/param/param_ExecuteOpcode.c \ + libcanard/dsdl/uavcan/protocol/param/param_GetSet.c \ + libcanard/dsdl/uavcan/protocol/param/param_NumericValue.c \ + libcanard/dsdl/uavcan/protocol/param/param_Value.c \ + libcanard/dsdl/uavcan/protocol/file/file_BeginFirmwareUpdate.c \ + libcanard/dsdl/uavcan/protocol/file/file_Read.c CANARDINC = libcanard \ libcanard/dsdl diff --git a/libcanard/canard_driver.c b/libcanard/canard_driver.c old mode 100644 new mode 100755 index 99b92f19..af9a47ea --- a/libcanard/canard_driver.c +++ b/libcanard/canard_driver.c @@ -4,17 +4,17 @@ This file is part of the VESC firmware. The VESC firmware is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. - The VESC firmware is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with this program. If not, see . + You should have received a copy of the GNU General Public License + along with this program. If not, see . */ #include @@ -26,6 +26,11 @@ #include "uavcan/equipment/esc/Status.h" #include "uavcan/equipment/esc/RawCommand.h" #include "uavcan/equipment/esc/RPMCommand.h" +#include "uavcan/protocol/param/GetSet.h" +#include "uavcan/protocol/GetNodeInfo.h" +#include "uavcan/protocol/RestartNode.h" +#include +#include #include "conf_general.h" #include "app.h" @@ -35,53 +40,57 @@ #include "hw.h" #include "timeout.h" #include "terminal.h" +#include "mempools.h" +#include "flash_helper.h" +#include "crc.h" +#include "nrf_driver.h" +#include "buffer.h" // Constants -#define APP_NODE_NAME "org.vesc." HW_NAME - -#define UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE ((3015 + 7) / 8) -#define UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE 0xee468a8121c46a9e -#define UAVCAN_GET_NODE_INFO_DATA_TYPE_ID 1 - -#define UAVCAN_NODE_STATUS_MESSAGE_SIZE 7 -#define UAVCAN_NODE_STATUS_DATA_TYPE_ID 341 -#define UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE 0x0f0868d0c1a7c6f1 - -#define UAVCAN_NODE_HEALTH_OK 0 -#define UAVCAN_NODE_HEALTH_WARNING 1 -#define UAVCAN_NODE_HEALTH_ERROR 2 -#define UAVCAN_NODE_HEALTH_CRITICAL 3 - -#define UAVCAN_NODE_MODE_OPERATIONAL 0 -#define UAVCAN_NODE_MODE_INITIALIZATION 1 - -#define UNIQUE_ID_LENGTH_BYTES 16 - +#define CAN_APP_NODE_NAME "org.vesc." HW_NAME +#define UNIQUE_ID_LENGTH_BYTES 12 #define STATUS_MSGS_TO_STORE 10 +#define AP_MAX_NAME_SIZE 16 +#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) + +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH 16 + +#define CURRENT_CALC_FREQ_HZ 10 +#define PARAM_REFRESH_RATE_HZ 10 +#define ESC_STATUS_TIMEOUT 500 //ms? +#define RESERVED_FLASH_SPACE_SIZE 393216 + +/* + * Node status variables + */ +static uavcan_protocol_NodeStatus node_status; // Private datatypes typedef struct { int id; systime_t rx_time; + bool timeout_flag; uavcan_equipment_esc_Status msg; } status_msg_wrapper_t; // Private variables static CanardInstance canard; static uint8_t canard_memory_pool[1024]; -static uint8_t node_health = UAVCAN_NODE_HEALTH_OK; -static uint8_t node_mode = UAVCAN_NODE_MODE_OPERATIONAL; +static uint8_t node_health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; +static uint8_t node_mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; static int debug_level; static status_msg_wrapper_t stat_msgs[STATUS_MSGS_TO_STORE]; +static uint8_t my_node_id = 0; +static bool refresh_parameters_enabled = true; // Threads static THD_WORKING_AREA(canard_thread_wa, 2048); static THD_FUNCTION(canard_thread, arg); // Private functions +static void calculateTotalCurrent(void); static void sendEscStatus(void); static void readUniqueID(uint8_t* out_uid); -static void makeNodeStatusMessage(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]); static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer); static bool shouldAcceptTransfer(const CanardInstance* ins, uint64_t* out_data_type_signature, @@ -90,6 +99,219 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, uint8_t source_node_id); static void terminal_debug_on(int argc, const char **argv); +/* +* Firmware Update Stuff +*/ +static struct { + uint64_t ofs; + uint32_t last_ms; + uint8_t node_id; + uint8_t transfer_id; + uint8_t path[UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH+1]; + uint8_t sector; + uint32_t sector_ofs; +} fw_update; + +systime_t last_read_file_req = 0; +systime_t jump_delay_start = 0; +bool jump_to_bootloader = false; + +#define FLASH_SECTORS 12 +#define BOOTLOADER_BASE 11 +#define APP_BASE 0 +#define APP_SECTORS 7 +#define NEW_APP_BASE 8 +#define NEW_APP_SECTORS 3 +#define NEW_APP_MAX_SIZE (3 * (1 << 17)) + +/* + * Base address of the Flash sectors + * May be able to remove these and only use the address for NEW_APP_BASE and APP_BASE instead + */ +#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000) // Base @ of Sector 0, 16 Kbytes +#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08004000) // Base @ of Sector 1, 16 Kbytes +#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08008000) // Base @ of Sector 2, 16 Kbytes +#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x0800C000) // Base @ of Sector 3, 16 Kbytes +#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08010000) // Base @ of Sector 4, 64 Kbytes +#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08020000) // Base @ of Sector 5, 128 Kbytes +#define ADDR_FLASH_SECTOR_6 ((uint32_t)0x08040000) // Base @ of Sector 6, 128 Kbytes +#define ADDR_FLASH_SECTOR_7 ((uint32_t)0x08060000) // Base @ of Sector 7, 128 Kbytes +#define ADDR_FLASH_SECTOR_8 ((uint32_t)0x08080000) // Base @ of Sector 8, 128 Kbytes +#define ADDR_FLASH_SECTOR_9 ((uint32_t)0x080A0000) // Base @ of Sector 9, 128 Kbytes +#define ADDR_FLASH_SECTOR_10 ((uint32_t)0x080C0000) // Base @ of Sector 10, 128 Kbytes +#define ADDR_FLASH_SECTOR_11 ((uint32_t)0x080E0000) // Base @ of Sector 11, 128 Kbytes + +static const uint32_t flash_addr[FLASH_SECTORS] = { + ADDR_FLASH_SECTOR_0, + ADDR_FLASH_SECTOR_1, + ADDR_FLASH_SECTOR_2, + ADDR_FLASH_SECTOR_3, + ADDR_FLASH_SECTOR_4, + ADDR_FLASH_SECTOR_5, + ADDR_FLASH_SECTOR_6, + ADDR_FLASH_SECTOR_7, + ADDR_FLASH_SECTOR_8, + ADDR_FLASH_SECTOR_9, + ADDR_FLASH_SECTOR_10, + ADDR_FLASH_SECTOR_11 +}; + +/* + * Parameter types and enums + */ +typedef struct +{ + char* name; + uint8_t type; + int64_t val; + int64_t min; + int64_t max; + int64_t defval; +} param_t; + +enum ap_var_type { + AP_PARAM_NONE = 0, + AP_PARAM_INT8, + AP_PARAM_INT16, + AP_PARAM_INT32, + AP_PARAM_FLOAT, + AP_PARAM_VECTOR3F, + AP_PARAM_GROUP +}; + +/* + * Private Parameter function declarations + */ +static void updateParamByName(uint8_t * name, float value); +static void refresh_parameters(void); +static param_t* getParamByIndex(uint16_t index); +static param_t* getParamByName(char * name); +static void write_app_config(void); + +/* + * Local parameter table + * This table contains the parameters we want to be able to change via UAVCAN. It is a copy of the + * appconf parameters with more information, the format of each parameter is as follows: + * { parameter name, parameter type, current value, min value, max value, default value} + */ +static param_t parameters[] = +{ + {"uavcan_mode", AP_PARAM_INT8, 0, 0, 2, 1}, + {"can_baud_rate", AP_PARAM_INT8, 0, 0, 8, 3}, + {"can_status_rate", AP_PARAM_INT32, 0, 0, 100, 50}, + {"can_send_status", AP_PARAM_INT8, 0, 0, 5, 1}, + {"can_esc_index", AP_PARAM_INT8, 0, 0, 255, 0}, + {"controller_id", AP_PARAM_INT8, 0, 0, 255, 0} +}; + +/* + * This function updates the local parameter value. It is called after reading the current appconf + * data to update the local copy of the parameter. + */ +static void updateParamByName(uint8_t * name, float value) +{ + param_t* p = NULL; + p = getParamByName((char *)name); + if (p != NULL) { + if(p->val != value) { + commands_printf("%s, %s p->val %0.02f, value %0.02f", p->name, name, (double)p->val, (double)value); + p->val = value; + } + } else { + commands_printf("UAVCAN updateParamByName(): Parameter name not found"); + } +} + +/* + * This function updates the app config from the local app config. It is used to + * write new parameters after a set parameter request. + */ +static void write_app_config(void) { + app_configuration *appconf = mempools_alloc_appconf(); + *appconf = *app_get_configuration(); + + appconf->can_mode = (uint8_t)getParamByName("uavcan_mode")->val; + appconf->can_baud_rate = (uint8_t)getParamByName("can_baud_rate")->val; + appconf->send_can_status_rate_hz = (uint32_t)getParamByName("can_status_rate")->val; + appconf->send_can_status = (uint8_t)getParamByName("can_send_status")->val; + appconf->uavcan_esc_index = (uint8_t)getParamByName("can_esc_index")->val; + appconf->controller_id = (uint8_t)getParamByName("controller_id")->val; + + conf_general_store_app_configuration(appconf); + app_set_configuration(appconf); + + mempools_free_appconf(appconf); + + refresh_parameters_enabled = true; +} + +/* + * This function updates the local copy of the parameters from the appconf data + * This is called periodically so that the paremters are kept in sync. Care has + * to be taken when a set parameter request is recieved so that the parameter is + * not overwritten before it is actually writen to the emulated EEPROM. + */ +static void refresh_parameters(void){ + app_configuration *appconf = mempools_alloc_appconf(); + *appconf = *app_get_configuration(); + + updateParamByName((uint8_t *)"uavcan_mode", appconf->can_mode ); + updateParamByName((uint8_t *)"can_baud_rate", appconf->can_baud_rate ); + updateParamByName((uint8_t *)"can_status_rate", appconf->send_can_status_rate_hz ); + updateParamByName((uint8_t *)"can_send_status", appconf->send_can_status ); + updateParamByName((uint8_t *)"can_esc_index", appconf->uavcan_esc_index ); + updateParamByName((uint8_t *)"controller_id", appconf->controller_id ); + + mempools_free_appconf(appconf); +} + +/* + * Get parameter by index + * Retrieves the parameter with the given index if found + * if not found it returns null + */ +static param_t* getParamByIndex(uint16_t index) +{ + if (index >= (sizeof(parameters)/40)) + { + if (debug_level == 2) { + commands_printf("Index is out of range"); + } + return NULL; + } + if (debug_level == 2) { + commands_printf("Size of parameter array is: %d",sizeof(parameters)/40); + } + return ¶meters[index]; +} + +/* + * Get parameter by name + * Searches the parameter table for the given name and returns the parameter information + * if no parameter is found it returns null. + */ +static param_t* getParamByName(char * name) +{ + for (uint16_t i = 0; i < sizeof(parameters)/40; i++) + { + if (debug_level == 2) { + commands_printf("name: %s paramname: %s", name, parameters[i].name); + } + + if (strcmp(name, parameters[i].name) == 0) + { + if (debug_level == 2) { + commands_printf("found match!"); + } + return ¶meters[i]; + } + } + return NULL; +} + +/* + * UAVCAN Driver Init + */ void canard_driver_init(void) { debug_level = 0; @@ -100,12 +322,79 @@ void canard_driver_init(void) { chThdCreateStatic(canard_thread_wa, sizeof(canard_thread_wa), NORMALPRIO, canard_thread, NULL); terminal_register_command_callback( - "uavcan_debug", - "Enable UAVCAN debug prints (0 = off)", - "[level]", - terminal_debug_on); + "uavcan_debug", + "Enable UAVCAN debug prints 0: off 1: errors 2: param getset 3: current calc 4: comms stuff)", + "[level]", + terminal_debug_on); } +/* + * Calculate the total system current consumption based on the reported consumption by other + * ESCs transmitting data on the bus. + */ +static void calculateTotalCurrent(void) { + // Calculate total current being consumed by the ESCs on the system + float totalCurrent = mc_interface_get_tot_current(); + float avgVoltage = GET_INPUT_VOLTAGE(); + float totalSysCurrent = 0; + + uint8_t escTotal = 1; + + for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) { + status_msg_wrapper_t *msgw = &stat_msgs[i]; + if (msgw->id != -1) { + systime_t elapsedTime = chVTGetSystemTimeX() - msgw->rx_time; + if(ST2MS(elapsedTime) > ESC_STATUS_TIMEOUT) { + commands_printf("ESC timeout for NodeID: %d",msgw->id); + msgw->id = -1; + } else { + totalCurrent += msgw->msg.current; + totalSysCurrent += msgw->msg.current; + escTotal++; + avgVoltage += msgw->msg.voltage; + } + } + } + + avgVoltage = avgVoltage / escTotal; + if(debug_level == 3) { + commands_printf("Total number of nodes: %d", escTotal); + commands_printf("Total Current: %0.02f", (double)totalCurrent); + commands_printf("Total Sys Curr: %0.02f", (double)totalSysCurrent); + commands_printf("Average Voltage: %0.02f", (double)avgVoltage); + } + + // ToDo: Add a way to limit the ESC current based on the total system current consumed +} + +/* +* Send Node Status Message +*/ +static void sendNodeStatus(void) { + node_mode = fw_update.node_id?UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + + + uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; + + node_status.health = node_health; + node_status.mode = node_mode; + node_status.uptime_sec = (uint32_t)ST2S(chVTGetSystemTimeX()); + // status.sub_mode = + // status.vendor_specific_status_code is filled in the firmware update loop + uavcan_protocol_NodeStatus_encode(&node_status, buffer); + static uint8_t transfer_id; + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE); +} + +/* + * Send ESC Status Message + */ static void sendEscStatus(void) { uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE]; uavcan_equipment_esc_Status status; @@ -123,91 +412,100 @@ static void sendEscStatus(void) { static uint8_t transfer_id; + if (debug_level > 11) { + commands_printf("UAVCAN sendESCStatus"); + } + canardBroadcast(&canard, - UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, - UAVCAN_EQUIPMENT_ESC_STATUS_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE); + UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE); } -static void readUniqueID(uint8_t* out_uid) { - for (uint8_t i = 0; i < UNIQUE_ID_LENGTH_BYTES; i++) { - out_uid[i] = i; - } -} - -static void makeNodeStatusMessage(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]) { - memset(buffer, 0, UAVCAN_NODE_STATUS_MESSAGE_SIZE); - const uint32_t uptime_sec = ST2S(chVTGetSystemTimeX()); - canardEncodeScalar(buffer, 0, 32, &uptime_sec); - canardEncodeScalar(buffer, 32, 2, &node_health); - canardEncodeScalar(buffer, 34, 3, &node_mode); -} - -/** - * This callback is invoked by the library when a new message or request or response is received. +/* + * Reads the STM32 Unique ID */ -static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) { - if (debug_level > 0) { - commands_printf("UAVCAN transfer RX: NODE: %d Type: %d ID: %d", - transfer->source_node_id, transfer->transfer_type, transfer->data_type_id); +static void readUniqueID(uint8_t* out_uid) { + uint8_t len = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH; + memset(out_uid, 0, len); + memcpy(out_uid, (const void *)STM32_UUID_8, UNIQUE_ID_LENGTH_BYTES); +} + +/* + * Handle a GET_NODE_INFO request + * FW_VERSION is from conf_general.h + * HW_VERSION is from HW_HEADER if HW_MAJOR/HW_MINOR are defined, else it sends 0 + * This needs a change to all HW_HEADER files if desired. + * Unique ID is now being read from the STM32 UUID field + * Node status is syncronize with the data from the node status message. + */ +static void handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer) { + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + uavcan_protocol_GetNodeInfoResponse pkt; + + node_status.uptime_sec = ST2S(chVTGetSystemTimeX()); + + pkt.status = node_status; + pkt.software_version.major = FW_VERSION_MAJOR; + pkt.software_version.minor = FW_VERSION_MINOR; + pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT | UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_IMAGE_CRC; + pkt.software_version.vcs_commit = FW_TEST_VERSION_NUMBER; + uint32_t *crc = (uint32_t *)&pkt.software_version.image_crc; + crc[0] = 0; + crc[1] = 0; + + readUniqueID(pkt.hardware_version.unique_id); + + // use hw major/minor for APJ_BOARD_ID so we know what fw is + // compatible with this hardware +#ifdef HW_MAJOR + pkt.hardware_version.major = HW_MAJOR; + pkt.hardware_version.minor = HW_MINOR; +#else + pkt.hardware_version.major = 0; + pkt.hardware_version.minor = 0; +#endif + + char name[strlen(CAN_APP_NODE_NAME)+1]; + strcpy(name, CAN_APP_NODE_NAME); + pkt.name.len = strlen(CAN_APP_NODE_NAME); + pkt.name.data = (uint8_t *)name; + + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + + const int16_t resp_res = canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); + if (resp_res <= 0) { + if (debug_level > 1) { + commands_printf("Could not respond to GetNodeInfo: %d\n", resp_res); + } } +} - if ((transfer->transfer_type == CanardTransferTypeRequest) && - (transfer->data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) { +/* + * Handle ESC Raw command + */ +static void handle_esc_raw_command(CanardInstance* ins, CanardRxTransfer* transfer) { + uavcan_equipment_esc_RawCommand cmd; + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE]; + memset(buffer, 0, sizeof(buffer)); + uint8_t *tmp = buffer; - uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]; - memset(buffer, 0, sizeof(buffer)); + if (uavcan_equipment_esc_RawCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0) >= 0) { + if (cmd.cmd.len > app_get_configuration()->uavcan_esc_index) { + float raw_val = ((float)cmd.cmd.data[app_get_configuration()->uavcan_esc_index]) / 8192.0; - // NodeStatus - makeNodeStatusMessage(buffer); - - // SoftwareVersion - buffer[7] = FW_VERSION_MAJOR; - buffer[8] = FW_VERSION_MINOR; - buffer[9] = 1; // Optional field flags, VCS commit is set - uint32_t u32 = 0;// GIT_HASH; - canardEncodeScalar(buffer, 80, 32, &u32); - // Image CRC skipped - - // HardwareVersion - // Major skipped - // Minor skipped - readUniqueID(&buffer[24]); - // Certificate of authenticity skipped - - // Name - const size_t name_len = strlen(APP_NODE_NAME); - memcpy(&buffer[41], APP_NODE_NAME, name_len); - - const size_t total_size = 41 + name_len; - - /* - * Transmitting; in this case we don't have to release the payload because it's empty anyway. - */ - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE, - UAVCAN_GET_NODE_INFO_DATA_TYPE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - (uint16_t)total_size); - } else if ((transfer->transfer_type == CanardTransferTypeBroadcast) && - (transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID)) { - uavcan_equipment_esc_RawCommand cmd; - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE]; - memset(buffer, 0, sizeof(buffer)); - uint8_t *tmp = buffer; - - if (uavcan_equipment_esc_RawCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0, true) >= 0) { - if (cmd.cmd.len > app_get_configuration()->uavcan_esc_index) { - float raw_val = ((float)cmd.cmd.data[app_get_configuration()->uavcan_esc_index]) / 8192.0; - - switch (app_get_configuration()->uavcan_raw_mode) { + switch (app_get_configuration()->uavcan_raw_mode) { case UAVCAN_RAW_MODE_CURRENT: mc_interface_set_current_rel(raw_val); break; @@ -223,42 +521,492 @@ static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) case UAVCAN_RAW_MODE_DUTY: mc_interface_set_duty(raw_val); break; + default: break; - } - timeout_reset(); } + timeout_reset(); } - } else if ((transfer->transfer_type == CanardTransferTypeBroadcast) && - (transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID)) { - uavcan_equipment_esc_RPMCommand cmd; - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_MAX_SIZE]; - memset(buffer, 0, sizeof(buffer)); - uint8_t *tmp = buffer; + } +} - if (uavcan_equipment_esc_RPMCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0, true) >= 0) { - if (cmd.rpm.len > app_get_configuration()->uavcan_esc_index) { - mc_interface_set_pid_speed(cmd.rpm.data[app_get_configuration()->uavcan_esc_index]); - timeout_reset(); - } +/* + * Handle ESC RPM command + */ +static void handle_esc_rpm_command(CanardInstance* ins, CanardRxTransfer* transfer) { + uavcan_equipment_esc_RPMCommand cmd; + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_MAX_SIZE]; + memset(buffer, 0, sizeof(buffer)); + uint8_t *tmp = buffer; + + if (uavcan_equipment_esc_RPMCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0) >= 0) { + if (cmd.rpm.len > app_get_configuration()->uavcan_esc_index) { + mc_interface_set_pid_speed(cmd.rpm.data[app_get_configuration()->uavcan_esc_index]); + timeout_reset(); } - } else if ((transfer->transfer_type == CanardTransferTypeBroadcast) && - (transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_STATUS_ID)) { - uavcan_equipment_esc_Status msg; - if (uavcan_equipment_esc_Status_decode_internal(transfer, transfer->payload_len, &msg, 0, 0, true) >= 0) { - for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) { - status_msg_wrapper_t *msgw = &stat_msgs[i]; - if (msgw->id == -1 || msgw->id == transfer->source_node_id) { - msgw->id = transfer->source_node_id; - msgw->rx_time = chVTGetSystemTimeX(); - msgw->msg = msg; - break; - } + } +} + +/* + * Handle Equipment ESC Status Request + * Reads status messages from other ESCs on the system and stores the information in memory + * this data can be used to respond to total system current consumption. + */ +static void handle_esc_status(CanardInstance* ins, CanardRxTransfer* transfer) { + uavcan_equipment_esc_Status msg; + if (uavcan_equipment_esc_Status_decode_internal(transfer, transfer->payload_len, &msg, 0, 0) >= 0) { + for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) { + status_msg_wrapper_t *msgw = &stat_msgs[i]; + if (msgw->id == -1 || msgw->id == transfer->source_node_id) { + msgw->id = transfer->source_node_id; + msgw->rx_time = chVTGetSystemTimeX(); + msgw->msg = msg; + break; } } } } +/* + * Handle parameter GetSet request + * In UAVCAN parameters are requested individually either by index or by name + * VESC currently does not have a way to write parameters individually or a way to + * access the parameters by name. So I needed to create a bridge between the parameters + * and the UAVCAN driver which was not very clean and creates too much overhead if we + * want to access a large number of parameters. For that reason only a few paremeters + * were added here to allow changing of the most basic features. + * For now any tunning that needs to be done could be done using the USB interface + * parameters can be changed and if it needs to be deployed on the field to other + * ESCs that are using a UAVCAN interface then a special firmware with the defaults + * modified can be create and deployed by a UAVCAN firmware update + */ +static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) +{ + uavcan_protocol_param_GetSetRequest req; + uint8_t arraybuf[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_NAME_MAX_LENGTH]; + uint8_t *arraybuf_ptr = arraybuf; + if (uavcan_protocol_param_GetSetRequest_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) { + return; + } + + uavcan_protocol_param_GetSetResponse pkt; + + memset(&pkt, 0, sizeof(uavcan_protocol_param_GetSetResponse)); + + char name[AP_MAX_NAME_SIZE+1] = ""; + + param_t* p = NULL; + + // Verify the request is valid and try to get the parameter if it is. + if (req.name.len != 0 && req.name.len > AP_MAX_NAME_SIZE) { + if (debug_level > 1) { + commands_printf("UAVCAN param_getset: Parameter Name is too long!"); + } + p = NULL; + } else if (req.name.len != 0 && req.name.len <= AP_MAX_NAME_SIZE) { + strncpy((char *)name, (char *)req.name.data, req.name.len); + if (debug_level > 1) { + commands_printf("UAVCAN param_getset param name: %s", name); + } + p = getParamByName(name); + } else { + if (debug_level > 1) { + commands_printf("UAVCAN param_getset param index: %d", req.index); + } + p = getParamByIndex(req.index); + } + + // If a valid parameter was retrived... + if ((p != NULL) && (debug_level > 1)) { + uint8_t arrSize = strlen(p->name); + commands_printf("UAVCAN param_getset got\nname: %s\nsize: %d\ntype: %d\nvalue: %d", (char *)p->name, arrSize, p->type, p->val); + } + + // If a valid parameter was retrived and this is a set parameter request... + if (p != NULL && req.name.len != 0 && req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY) { + // Set request and valid parameter found + // Prevent overwrite of local parameter copy before its written to eeprom + refresh_parameters_enabled = false; + + switch (req.value.union_tag) { + case UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY: + return; + break; + + case UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE: + switch(p->type) { + case AP_PARAM_INT8: + p->val = (uint8_t)req.value.integer_value; + break; + + case AP_PARAM_INT16: + p->val = (uint16_t)req.value.integer_value; + break; + + case AP_PARAM_INT32: + p->val = (uint32_t)req.value.integer_value; + break; + + case AP_PARAM_FLOAT: + p->val = (float)req.value.integer_value; + break; + + default: + break; + } + break; + + default: + break; + } + write_app_config(); + } + + // If a valid parameter was retrived send back the value + if(p != NULL) { + switch(p->type) { + case AP_PARAM_INT8: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.value.integer_value = (int8_t)p->val; + pkt.default_value.integer_value = (int8_t)p->defval; + pkt.min_value.integer_value = (int8_t)p->min; + pkt.max_value.integer_value = (int8_t)p->max; + break; + + case AP_PARAM_INT16: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.value.integer_value = (int16_t)p->val; + pkt.default_value.integer_value = (int16_t)p->defval; + pkt.min_value.integer_value = (int16_t)p->min; + pkt.max_value.integer_value = (int16_t)p->max; + break; + + case AP_PARAM_INT32: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + pkt.value.integer_value = (int32_t)p->val; + pkt.default_value.integer_value = (int32_t)p->defval; + pkt.min_value.integer_value = (int32_t)p->min; + pkt.max_value.integer_value = (int32_t)p->max; + break; + + case AP_PARAM_FLOAT: + pkt.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + pkt.default_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + pkt.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + pkt.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + pkt.value.real_value = (float)p->val; + pkt.default_value.real_value = (float)p->defval; + pkt.min_value.real_value = (float)p->min; + pkt.max_value.real_value = (float)p->max; + break; + } + + pkt.name.len = strlen((char *)p->name); + pkt.name.data = (uint8_t *)p->name; + } + + if (debug_level == 2) { + commands_printf("UAVCAN param_getset: Sending %s", pkt.name.data); + } + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE]; + uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer); + + const int16_t resp_res = canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_GETSET_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); + + if ((resp_res <= 0) && (debug_level > 1)) { + commands_printf("Could not respond to param_getset_req: %d\n", resp_res); + } +} + +/* + * Create a Watchdog reset in order to restart the node if a restart command is recieved + */ +static void handle_restart_node(void) { + // Lock the system and enter an infinite loop. The watchdog will reboot. + __disable_irq(); + for(;;){}; +} + +/* + * Send a read for a fw update file + * This request is sent after we recieve a begin firmware udpate request, and then + * everytime we finish processing one chunk of the firmware file. It uses the + * fw_update.ofs value to keep track of what chunk of data needs to be requested. + * We use fw_update.last_ms to give the server enough time to respond to the previous + * request before we send a new one. The value is cleared after procesing a response + * so that we dont have to wait 250ms before sending the next request. + */ +static void send_fw_read(void) +{ + uint32_t now = ST2MS(chVTGetSystemTimeX()); + if (now - fw_update.last_ms < 250) { + // the server may still be responding + return; + } + fw_update.last_ms = now; + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; + canardEncodeScalar(buffer, 0, 40, &fw_update.ofs); + uint32_t offset = 40; + uint8_t len = strlen((const char *)fw_update.path); + for (uint8_t i=0; itransfer_id+1)%256 != fw_update.transfer_id || + transfer->source_node_id != fw_update.node_id) { + return; + } + int16_t error = 0; + canardDecodeScalar(transfer, 0, 16, true, (void*)&error); + uint16_t len = transfer->payload_len - 2; + + uint32_t offset = 16; + uint32_t buf32[(len+3)/4]; + uint8_t *buf = (uint8_t *)&buf32[0]; + for (uint16_t i=0; ipayload_len < 1 || transfer->payload_len > sizeof(fw_update.path)+1) { + return; + } + + if (fw_update.node_id == 0) { + uint32_t offset = 0; + canardDecodeScalar(transfer, 0, 8, false, (void*)&fw_update.node_id); + offset += 8; + for (uint8_t i=0; ipayload_len-1; i++) { + canardDecodeScalar(transfer, offset, 8, false, (void*)&fw_update.path[i]); + offset += 8; + } + + fw_update.ofs = 0; + fw_update.last_ms = 0; + fw_update.sector = 0; + fw_update.sector_ofs = 0; + if (fw_update.node_id == 0) { + last_read_file_req = chVTGetSystemTimeX(); + fw_update.node_id = transfer->source_node_id; + } + } + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; + uavcan_protocol_file_BeginFirmwareUpdateResponse reply; + reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; + + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); + + // Erase the reserved flash for new app + flash_helper_erase_new_app(RESERVED_FLASH_SPACE_SIZE); + + last_read_file_req = chVTGetSystemTimeX(); + + if (debug_level > 0) { + commands_printf("UAVCAN Begin firmware update from node_id: %d",fw_update.node_id); + } + + send_fw_read(); +} + +/** +* This callback is invoked by the library when a new message or request or response is received. +*/ +static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) { + if (debug_level == 4) { + commands_printf("UAVCAN transfer RX: NODE: %d Type: %d ID: %d", + transfer->source_node_id, transfer->transfer_type, transfer->data_type_id); + } + + /* + * Dynamic node ID allocation protocol. + * Taking this branch only if we don't have a node ID, ignoring otherwise. + */ + // if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { + // if (transfer->transfer_type == CanardTransferTypeBroadcast && + // transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) { + // handle_allocation_response(ins, transfer); + // } + // return; + // } + + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + handle_get_node_info(ins, transfer); + break; + + case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: + handle_esc_raw_command(ins, transfer); + break; + + case UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID: + handle_esc_rpm_command(ins, transfer); + break; + + case UAVCAN_EQUIPMENT_ESC_STATUS_ID: + handle_esc_status(ins, transfer); + break; + + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + commands_printf("RestartNode\n"); + handle_restart_node(); + break; + + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: + handle_param_getset(ins, transfer); + break; + + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: + handle_begin_firmware_update(ins, transfer); + break; + + case UAVCAN_PROTOCOL_FILE_READ_ID: + handle_file_read_response(ins, transfer); + break; + } +} + /** * This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received * by the local node. @@ -267,36 +1015,68 @@ static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) * All transfers that are addressed to other nodes are always ignored. */ static bool shouldAcceptTransfer(const CanardInstance* ins, - uint64_t* out_data_type_signature, - uint16_t data_type_id, - CanardTransferType transfer_type, - uint8_t source_node_id) { - (void)ins; + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id) +{ (void)source_node_id; - if (debug_level > 0) { + if (debug_level == 4) { commands_printf("UAVCAN shouldAccept: NODE: %d Type: %d ID: %d", source_node_id, transfer_type, data_type_id); } - if ((transfer_type == CanardTransferTypeRequest) && (data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) { - *out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE; - return true; - } + // This is for future use if Dynamic node ID allocation is used. + // if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) + // { + // /* + // * If we're in the process of allocation of dynamic node ID, accept only relevant transfers. + // */ + // if ((transfer_type == CanardTransferTypeBroadcast) && + // (data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID)) + // { + // *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; + // return true; + // } + // return false; + // } - if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID)) { - *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE; - return true; - } + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE; + return true; - if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID)) { - *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_SIGNATURE; - return true; - } + case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE; + return true; - if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_STATUS_ID)) { - *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE; - return true; + case UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_SIGNATURE; + return true; + + case UAVCAN_EQUIPMENT_ESC_STATUS_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE; + return true; + + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; + return true; + + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + return true; + + case UAVCAN_PROTOCOL_FILE_READ_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE; + return true; + + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; + return true; + + default: + break; } return false; @@ -326,7 +1106,9 @@ static THD_FUNCTION(canard_thread, arg) { systime_t last_status_time = 0; systime_t last_esc_status_time = 0; - + systime_t last_tot_current_calc_time = 0; + systime_t last_param_refresh = 0; + for (;;) { const app_configuration *conf = app_get_configuration(); @@ -335,6 +1117,7 @@ static THD_FUNCTION(canard_thread, arg) { continue; } + my_node_id = conf->controller_id; canardSetLocalNodeID(&canard, conf->controller_id); CANRxFrame *rxmsg; @@ -361,26 +1144,46 @@ static THD_FUNCTION(canard_thread, arg) { if (ST2MS(chVTTimeElapsedSinceX(last_status_time)) >= 1000) { last_status_time = chVTGetSystemTimeX(); canardCleanupStaleTransfers(&canard, ST2US(chVTGetSystemTimeX())); - - uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]; - makeNodeStatusMessage(buffer); - - static uint8_t transfer_id; - canardBroadcast(&canard, - UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE, - UAVCAN_NODE_STATUS_DATA_TYPE_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - UAVCAN_NODE_STATUS_MESSAGE_SIZE); + sendNodeStatus(); } - if (ST2MS(chVTTimeElapsedSinceX(last_esc_status_time)) >= 1000 / conf->send_can_status_rate_hz && - conf->send_can_status != CAN_STATUS_DISABLED) { + if (ST2MS(chVTTimeElapsedSinceX(last_esc_status_time)) >= 1000 / conf->send_can_status_rate_hz && conf->send_can_status != CAN_STATUS_DISABLED) { last_esc_status_time = chVTGetSystemTimeX(); sendEscStatus(); } + if (ST2MS(chVTTimeElapsedSinceX(last_tot_current_calc_time)) >= 1000 / CURRENT_CALC_FREQ_HZ) { + last_tot_current_calc_time = chVTGetSystemTimeX(); + calculateTotalCurrent(); + if(debug_level == 3) { + mc_configuration *mcconf = mempools_alloc_mcconf(); + *mcconf = *mc_interface_get_configuration(); + commands_printf("Max Current: %0.02f",(double)mcconf->lo_current_motor_max_now); + mempools_free_mcconf(mcconf); + } + } + + if (ST2MS(chVTTimeElapsedSinceX(last_param_refresh)) >= 1000 / PARAM_REFRESH_RATE_HZ) { + last_param_refresh = chVTGetSystemTimeX(); + if(refresh_parameters_enabled) { + refresh_parameters(); + } + + if(debug_level == 7) { + commands_printf("Refreshing Parameters Time Delta: %d", ST2MS(chVTGetSystemTimeX()-last_param_refresh)); + } + } + + if ((ST2MS(chVTTimeElapsedSinceX(last_read_file_req)) >= 10) && (fw_update.node_id != 0)) { + last_read_file_req = chVTGetSystemTimeX(); + send_fw_read(); + } + + // delay jump to bootloader after receiving data for 0.5 sec + if ((ST2MS(chVTTimeElapsedSinceX(jump_delay_start)) >= 500) && (jump_to_bootloader == true)) { + flash_helper_jump_to_bootloader(); + } + chThdSleepMilliseconds(1); } } diff --git a/libcanard/dsdl/uavcan/equipment/esc/RPMCommand.h b/libcanard/dsdl/uavcan/equipment/esc/RPMCommand.h index 25d5443f..a72f713c 100644 --- a/libcanard/dsdl/uavcan/equipment/esc/RPMCommand.h +++ b/libcanard/dsdl/uavcan/equipment/esc/RPMCommand.h @@ -3,7 +3,7 @@ * * Autogenerated, do not edit. * - * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1031.RPMCommand.uavcan + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/equipment/esc/1031.RPMCommand.uavcan */ #ifndef __UAVCAN_EQUIPMENT_ESC_RPMCOMMAND @@ -64,7 +64,7 @@ extern uint32_t uavcan_equipment_esc_RPMCommand_encode_internal(uavcan_equipment_esc_RPMCommand* source, void* msg_buf, uint32_t offset, uint8_t root_item); extern -int32_t uavcan_equipment_esc_RPMCommand_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RPMCommand* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao); +int32_t uavcan_equipment_esc_RPMCommand_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RPMCommand* dest, uint8_t** dyn_arr_buf, int32_t offset); #ifdef __cplusplus } // extern "C" diff --git a/libcanard/dsdl/uavcan/equipment/esc/RawCommand.h b/libcanard/dsdl/uavcan/equipment/esc/RawCommand.h index af00d0f7..157f1a0e 100644 --- a/libcanard/dsdl/uavcan/equipment/esc/RawCommand.h +++ b/libcanard/dsdl/uavcan/equipment/esc/RawCommand.h @@ -3,7 +3,7 @@ * * Autogenerated, do not edit. * - * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1030.RawCommand.uavcan + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan */ #ifndef __UAVCAN_EQUIPMENT_ESC_RAWCOMMAND @@ -63,7 +63,7 @@ extern uint32_t uavcan_equipment_esc_RawCommand_encode_internal(uavcan_equipment_esc_RawCommand* source, void* msg_buf, uint32_t offset, uint8_t root_item); extern -int32_t uavcan_equipment_esc_RawCommand_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RawCommand* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao); +int32_t uavcan_equipment_esc_RawCommand_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RawCommand* dest, uint8_t** dyn_arr_buf, int32_t offset); #ifdef __cplusplus } // extern "C" diff --git a/libcanard/dsdl/uavcan/equipment/esc/Status.h b/libcanard/dsdl/uavcan/equipment/esc/Status.h index d877b4a0..c85c18d8 100644 --- a/libcanard/dsdl/uavcan/equipment/esc/Status.h +++ b/libcanard/dsdl/uavcan/equipment/esc/Status.h @@ -3,7 +3,7 @@ * * Autogenerated, do not edit. * - * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1034.Status.uavcan + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/equipment/esc/1034.Status.uavcan */ #ifndef __UAVCAN_EQUIPMENT_ESC_STATUS @@ -78,7 +78,7 @@ extern uint32_t uavcan_equipment_esc_Status_encode_internal(uavcan_equipment_esc_Status* source, void* msg_buf, uint32_t offset, uint8_t root_item); extern -int32_t uavcan_equipment_esc_Status_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_Status* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao); +int32_t uavcan_equipment_esc_Status_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_Status* dest, uint8_t** dyn_arr_buf, int32_t offset); #ifdef __cplusplus } // extern "C" diff --git a/libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c b/libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c index ce4c19e7..8999068d 100644 --- a/libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c +++ b/libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c @@ -3,7 +3,7 @@ * * Autogenerated, do not edit. * - * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1031.RPMCommand.uavcan + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/equipment/esc/1031.RPMCommand.uavcan */ #include "uavcan/equipment/esc/RPMCommand.h" #include "canard.h" @@ -13,12 +13,9 @@ #endif #ifndef CANARD_INTERNAL_SATURATE_UNSIGNED -#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) ); +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); #endif -#define CANARD_INTERNAL_ENABLE_TAO ((uint8_t) 1) -#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0) - #if defined(__GNUC__) # define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) #else @@ -85,7 +82,6 @@ uint32_t uavcan_equipment_esc_RPMCommand_encode(uavcan_equipment_esc_RPMCommand* * uavcan_equipment_esc_RPMCommand dyn memory will point to dyn_arr_buf memory. * NULL will ignore dynamic arrays decoding. * @param offset: Call with 0, bit offset to msg storage - * @param tao: is tail array optimization used * @retval offset or ERROR value if < 0 */ int32_t uavcan_equipment_esc_RPMCommand_decode_internal( @@ -93,15 +89,14 @@ int32_t uavcan_equipment_esc_RPMCommand_decode_internal( uint16_t CANARD_MAYBE_UNUSED(payload_len), uavcan_equipment_esc_RPMCommand* dest, uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), - int32_t offset, - uint8_t CANARD_MAYBE_UNUSED(tao)) + int32_t offset) { int32_t ret = 0; uint32_t c = 0; // Dynamic Array (rpm) // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization - if (payload_len && tao == CANARD_INTERNAL_ENABLE_TAO) + if (payload_len) { // - Calculate Array length from MSG length dest->rpm.len = ((payload_len * 8) - offset ) / 18; // 18 bit array item size @@ -110,7 +105,7 @@ int32_t uavcan_equipment_esc_RPMCommand_decode_internal( { // - Array length 5 bits ret = canardDecodeScalar(transfer, - offset, + (uint32_t)offset, 5, false, (void*)&dest->rpm.len); // 131071 @@ -132,7 +127,7 @@ int32_t uavcan_equipment_esc_RPMCommand_decode_internal( if (dyn_arr_buf) { ret = canardDecodeScalar(transfer, - offset, + (uint32_t)offset, 18, true, (void*)*dyn_arr_buf); // 131071 @@ -175,33 +170,13 @@ int32_t uavcan_equipment_esc_RPMCommand_decode(const CanardRxTransfer* transfer, const int32_t offset = 0; int32_t ret = 0; - /* Backward compatibility support for removing TAO - * - first try to decode with TAO DISABLED - * - if it fails fall back to TAO ENABLED - */ - uint8_t tao = CANARD_INTERNAL_DISABLE_TAO; - - while (1) + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_RPMCommand); c++) { - // Clear the destination struct - for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_RPMCommand); c++) - { - ((uint8_t*)dest)[c] = 0x00; - } - - ret = uavcan_equipment_esc_RPMCommand_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao); - - if (ret >= 0) - { - break; - } - - if (tao == CANARD_INTERNAL_ENABLE_TAO) - { - break; - } - tao = CANARD_INTERNAL_ENABLE_TAO; + ((uint8_t*)dest)[c] = 0x00; } + ret = uavcan_equipment_esc_RPMCommand_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + return ret; } diff --git a/libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c b/libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c index 5781ee05..001f4875 100644 --- a/libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c +++ b/libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c @@ -3,7 +3,7 @@ * * Autogenerated, do not edit. * - * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1030.RawCommand.uavcan + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan */ #include "uavcan/equipment/esc/RawCommand.h" #include "canard.h" @@ -13,12 +13,9 @@ #endif #ifndef CANARD_INTERNAL_SATURATE_UNSIGNED -#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) ); +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); #endif -#define CANARD_INTERNAL_ENABLE_TAO ((uint8_t) 1) -#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0) - #if defined(__GNUC__) # define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) #else @@ -85,7 +82,6 @@ uint32_t uavcan_equipment_esc_RawCommand_encode(uavcan_equipment_esc_RawCommand* * uavcan_equipment_esc_RawCommand dyn memory will point to dyn_arr_buf memory. * NULL will ignore dynamic arrays decoding. * @param offset: Call with 0, bit offset to msg storage - * @param tao: is tail array optimization used * @retval offset or ERROR value if < 0 */ int32_t uavcan_equipment_esc_RawCommand_decode_internal( @@ -93,15 +89,14 @@ int32_t uavcan_equipment_esc_RawCommand_decode_internal( uint16_t CANARD_MAYBE_UNUSED(payload_len), uavcan_equipment_esc_RawCommand* dest, uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), - int32_t offset, - uint8_t CANARD_MAYBE_UNUSED(tao)) + int32_t offset) { int32_t ret = 0; uint32_t c = 0; // Dynamic Array (cmd) // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization - if (payload_len && tao == CANARD_INTERNAL_ENABLE_TAO) + if (payload_len) { // - Calculate Array length from MSG length dest->cmd.len = ((payload_len * 8) - offset ) / 14; // 14 bit array item size @@ -110,7 +105,7 @@ int32_t uavcan_equipment_esc_RawCommand_decode_internal( { // - Array length 5 bits ret = canardDecodeScalar(transfer, - offset, + (uint32_t)offset, 5, false, (void*)&dest->cmd.len); // 8191 @@ -132,7 +127,7 @@ int32_t uavcan_equipment_esc_RawCommand_decode_internal( if (dyn_arr_buf) { ret = canardDecodeScalar(transfer, - offset, + (uint32_t)offset, 14, true, (void*)*dyn_arr_buf); // 8191 @@ -175,33 +170,13 @@ int32_t uavcan_equipment_esc_RawCommand_decode(const CanardRxTransfer* transfer, const int32_t offset = 0; int32_t ret = 0; - /* Backward compatibility support for removing TAO - * - first try to decode with TAO DISABLED - * - if it fails fall back to TAO ENABLED - */ - uint8_t tao = CANARD_INTERNAL_DISABLE_TAO; - - while (1) + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_RawCommand); c++) { - // Clear the destination struct - for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_RawCommand); c++) - { - ((uint8_t*)dest)[c] = 0x00; - } - - ret = uavcan_equipment_esc_RawCommand_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao); - - if (ret >= 0) - { - break; - } - - if (tao == CANARD_INTERNAL_ENABLE_TAO) - { - break; - } - tao = CANARD_INTERNAL_ENABLE_TAO; + ((uint8_t*)dest)[c] = 0x00; } + ret = uavcan_equipment_esc_RawCommand_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + return ret; } diff --git a/libcanard/dsdl/uavcan/equipment/esc/esc_Status.c b/libcanard/dsdl/uavcan/equipment/esc/esc_Status.c index 74adf45d..573b51de 100644 --- a/libcanard/dsdl/uavcan/equipment/esc/esc_Status.c +++ b/libcanard/dsdl/uavcan/equipment/esc/esc_Status.c @@ -3,7 +3,7 @@ * * Autogenerated, do not edit. * - * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1034.Status.uavcan + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/equipment/esc/1034.Status.uavcan */ #include "uavcan/equipment/esc/Status.h" #include "canard.h" @@ -13,12 +13,9 @@ #endif #ifndef CANARD_INTERNAL_SATURATE_UNSIGNED -#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) ); +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); #endif -#define CANARD_INTERNAL_ENABLE_TAO ((uint8_t) 1) -#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0) - #if defined(__GNUC__) # define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) #else @@ -112,7 +109,6 @@ uint32_t uavcan_equipment_esc_Status_encode(uavcan_equipment_esc_Status* source, * uavcan_equipment_esc_Status dyn memory will point to dyn_arr_buf memory. * NULL will ignore dynamic arrays decoding. * @param offset: Call with 0, bit offset to msg storage - * @param tao: is tail array optimization used * @retval offset or ERROR value if < 0 */ int32_t uavcan_equipment_esc_Status_decode_internal( @@ -120,8 +116,7 @@ int32_t uavcan_equipment_esc_Status_decode_internal( uint16_t CANARD_MAYBE_UNUSED(payload_len), uavcan_equipment_esc_Status* dest, uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), - int32_t offset, - uint8_t CANARD_MAYBE_UNUSED(tao)) + int32_t offset) { int32_t ret = 0; #ifndef CANARD_USE_FLOAT16_CAST @@ -130,7 +125,7 @@ int32_t uavcan_equipment_esc_Status_decode_internal( CANARD_USE_FLOAT16_CAST tmp_float = 0; #endif - ret = canardDecodeScalar(transfer, offset, 32, false, (void*)&dest->error_count); + ret = canardDecodeScalar(transfer, (uint32_t)offset, 32, false, (void*)&dest->error_count); if (ret != 32) { goto uavcan_equipment_esc_Status_error_exit; @@ -138,7 +133,7 @@ int32_t uavcan_equipment_esc_Status_decode_internal( offset += 32; // float16 special handling - ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float); + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); if (ret != 16) { @@ -152,7 +147,7 @@ int32_t uavcan_equipment_esc_Status_decode_internal( offset += 16; // float16 special handling - ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float); + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); if (ret != 16) { @@ -166,7 +161,7 @@ int32_t uavcan_equipment_esc_Status_decode_internal( offset += 16; // float16 special handling - ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float); + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); if (ret != 16) { @@ -179,21 +174,21 @@ int32_t uavcan_equipment_esc_Status_decode_internal( #endif offset += 16; - ret = canardDecodeScalar(transfer, offset, 18, true, (void*)&dest->rpm); + ret = canardDecodeScalar(transfer, (uint32_t)offset, 18, true, (void*)&dest->rpm); if (ret != 18) { goto uavcan_equipment_esc_Status_error_exit; } offset += 18; - ret = canardDecodeScalar(transfer, offset, 7, false, (void*)&dest->power_rating_pct); + ret = canardDecodeScalar(transfer, (uint32_t)offset, 7, false, (void*)&dest->power_rating_pct); if (ret != 7) { goto uavcan_equipment_esc_Status_error_exit; } offset += 7; - ret = canardDecodeScalar(transfer, offset, 5, false, (void*)&dest->esc_index); + ret = canardDecodeScalar(transfer, (uint32_t)offset, 5, false, (void*)&dest->esc_index); if (ret != 5) { goto uavcan_equipment_esc_Status_error_exit; @@ -230,33 +225,13 @@ int32_t uavcan_equipment_esc_Status_decode(const CanardRxTransfer* transfer, const int32_t offset = 0; int32_t ret = 0; - /* Backward compatibility support for removing TAO - * - first try to decode with TAO DISABLED - * - if it fails fall back to TAO ENABLED - */ - uint8_t tao = CANARD_INTERNAL_DISABLE_TAO; - - while (1) + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_Status); c++) { - // Clear the destination struct - for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_Status); c++) - { - ((uint8_t*)dest)[c] = 0x00; - } - - ret = uavcan_equipment_esc_Status_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao); - - if (ret >= 0) - { - break; - } - - if (tao == CANARD_INTERNAL_ENABLE_TAO) - { - break; - } - tao = CANARD_INTERNAL_ENABLE_TAO; + ((uint8_t*)dest)[c] = 0x00; } + ret = uavcan_equipment_esc_Status_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + return ret; } diff --git a/libcanard/dsdl/uavcan/protocol/GetNodeInfo.h b/libcanard/dsdl/uavcan/protocol/GetNodeInfo.h new file mode 100755 index 00000000..69bd393b --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/GetNodeInfo.h @@ -0,0 +1,120 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/AION/ardupilot_private/modules/uavcan/dsdl/uavcan/protocol/1.GetNodeInfo.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_GETNODEINFO +#define __UAVCAN_PROTOCOL_GETNODEINFO + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +/******************************* Source text ********************************** +# +# Full node info request. +# Note that all fields of the response section are byte-aligned. +# + +--- + +# +# Current node status +# +NodeStatus status + +# +# Version information shall not be changed while the node is running. +# +SoftwareVersion software_version +HardwareVersion hardware_version + +# +# Human readable non-empty ASCII node name. +# Node name shall not be changed while the node is running. +# Empty string is not a valid node name. +# Allowed characters are: a-z (lowercase ASCII letters) 0-9 (decimal digits) . (dot) - (dash) _ (underscore). +# Node name is a reversed internet domain name (like Java packages), e.g. "com.manufacturer.project.product". +# +uint8[<=80] name +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.GetNodeInfo +--- +uavcan.protocol.NodeStatus status +uavcan.protocol.SoftwareVersion software_version +uavcan.protocol.HardwareVersion hardware_version +saturated uint8[<=80] name +******************************************************************************/ + +#define UAVCAN_PROTOCOL_GETNODEINFO_ID 1 +#define UAVCAN_PROTOCOL_GETNODEINFO_NAME "uavcan.protocol.GetNodeInfo" +#define UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE (0xEE468A8121C46A9EULL) + +#define UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_MAX_SIZE ((0 + 7)/8) + +typedef struct +{ + uint8_t empty; +} uavcan_protocol_GetNodeInfoRequest; + +extern +uint32_t uavcan_protocol_GetNodeInfoRequest_encode(uavcan_protocol_GetNodeInfoRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_GetNodeInfoRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_GetNodeInfoRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_GetNodeInfoRequest_encode_internal(uavcan_protocol_GetNodeInfoRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_GetNodeInfoRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_GetNodeInfoRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE ((3015 + 7)/8) + +// Constants + +#define UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_NAME_MAX_LENGTH 80 + +typedef struct +{ + // FieldTypes + uavcan_protocol_NodeStatus status; // + uavcan_protocol_SoftwareVersion software_version; // + uavcan_protocol_HardwareVersion hardware_version; // + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[80] max items + } name; + +} uavcan_protocol_GetNodeInfoResponse; + +extern +uint32_t uavcan_protocol_GetNodeInfoResponse_encode(uavcan_protocol_GetNodeInfoResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_GetNodeInfoResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_GetNodeInfoResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_GetNodeInfoResponse_encode_internal(uavcan_protocol_GetNodeInfoResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_GetNodeInfoResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_GetNodeInfoResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_GETNODEINFO \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/HardwareVersion.h b/libcanard/dsdl/uavcan/protocol/HardwareVersion.h new file mode 100644 index 00000000..d568cec4 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/HardwareVersion.h @@ -0,0 +1,93 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/HardwareVersion.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_HARDWAREVERSION +#define __UAVCAN_PROTOCOL_HARDWAREVERSION + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Nested type. +# Generic hardware version information. +# These values should remain unchanged for the device's lifetime. +# + +# +# Hardware version code. +# +uint8 major +uint8 minor + +# +# Unique ID is a 128 bit long sequence that is globally unique for each node. +# All zeros is not a valid UID. +# If filled with zeros, assume that the value is undefined. +# +uint8[16] unique_id + +# +# Certificate of authenticity (COA) of the hardware, 255 bytes max. +# +uint8[<=255] certificate_of_authenticity +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.HardwareVersion +saturated uint8 major +saturated uint8 minor +saturated uint8[16] unique_id +saturated uint8[<=255] certificate_of_authenticity +******************************************************************************/ + +#define UAVCAN_PROTOCOL_HARDWAREVERSION_NAME "uavcan.protocol.HardwareVersion" +#define UAVCAN_PROTOCOL_HARDWAREVERSION_SIGNATURE (0xAD5C4C933F4A0C4ULL) + +#define UAVCAN_PROTOCOL_HARDWAREVERSION_MAX_SIZE ((2192 + 7)/8) + +// Constants + +#define UAVCAN_PROTOCOL_HARDWAREVERSION_UNIQUE_ID_LENGTH 16 +#define UAVCAN_PROTOCOL_HARDWAREVERSION_CERTIFICATE_OF_AUTHENTICITY_MAX_LENGTH 255 + +typedef struct +{ + // FieldTypes + uint8_t major; // bit len 8 + uint8_t minor; // bit len 8 + uint8_t unique_id[16]; // Static Array 8bit[16] max items + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[255] max items + } certificate_of_authenticity; + +} uavcan_protocol_HardwareVersion; + +extern +uint32_t uavcan_protocol_HardwareVersion_encode(uavcan_protocol_HardwareVersion* source, void* msg_buf); + +extern +int32_t uavcan_protocol_HardwareVersion_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_HardwareVersion* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_HardwareVersion_encode_internal(uavcan_protocol_HardwareVersion* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_HardwareVersion_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_HardwareVersion* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_HARDWAREVERSION \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/NodeStatus.h b/libcanard/dsdl/uavcan/protocol/NodeStatus.h new file mode 100755 index 00000000..13c3d81d --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/NodeStatus.h @@ -0,0 +1,136 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/AION/ardupilot_private/modules/uavcan/dsdl/uavcan/protocol/341.NodeStatus.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_NODESTATUS +#define __UAVCAN_PROTOCOL_NODESTATUS + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Abstract node status information. +# +# All UAVCAN nodes are required to publish this message periodically. +# + +# +# Publication period may vary within these limits. +# It is NOT recommended to change it at run time. +# +uint16 MAX_BROADCASTING_PERIOD_MS = 1000 +uint16 MIN_BROADCASTING_PERIOD_MS = 2 + +# +# If a node fails to publish this message in this amount of time, it should be considered offline. +# +uint16 OFFLINE_TIMEOUT_MS = 3000 + +# +# Uptime counter should never overflow. +# Other nodes may detect that a remote node has restarted when this value goes backwards. +# +uint32 uptime_sec + +# +# Abstract node health. +# +uint2 HEALTH_OK = 0 # The node is functioning properly. +uint2 HEALTH_WARNING = 1 # A critical parameter went out of range or the node encountered a minor failure. +uint2 HEALTH_ERROR = 2 # The node encountered a major failure. +uint2 HEALTH_CRITICAL = 3 # The node suffered a fatal malfunction. +uint2 health + +# +# Current mode. +# +# Mode OFFLINE can be actually reported by the node to explicitly inform other network +# participants that the sending node is about to shutdown. In this case other nodes will not +# have to wait OFFLINE_TIMEOUT_MS before they detect that the node is no longer available. +# +# Reserved values can be used in future revisions of the specification. +# +uint3 MODE_OPERATIONAL = 0 # Normal operating mode. +uint3 MODE_INITIALIZATION = 1 # Initialization is in progress; this mode is entered immediately after startup. +uint3 MODE_MAINTENANCE = 2 # E.g. calibration, the bootloader is running, etc. +uint3 MODE_SOFTWARE_UPDATE = 3 # New software/firmware is being loaded. +uint3 MODE_OFFLINE = 7 # The node is no longer available. +uint3 mode + +# +# Not used currently, keep zero when publishing, ignore when receiving. +# +uint3 sub_mode + +# +# Optional, vendor-specific node status code, e.g. a fault code or a status bitmask. +# +uint16 vendor_specific_status_code +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.NodeStatus +saturated uint32 uptime_sec +saturated uint2 health +saturated uint3 mode +saturated uint3 sub_mode +saturated uint16 vendor_specific_status_code +******************************************************************************/ + +#define UAVCAN_PROTOCOL_NODESTATUS_ID 341 +#define UAVCAN_PROTOCOL_NODESTATUS_NAME "uavcan.protocol.NodeStatus" +#define UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE (0xF0868D0C1A7C6F1ULL) + +#define UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE ((56 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_NODESTATUS_MAX_BROADCASTING_PERIOD_MS 1000 // 1000 +#define UAVCAN_PROTOCOL_NODESTATUS_MIN_BROADCASTING_PERIOD_MS 2 // 2 +#define UAVCAN_PROTOCOL_NODESTATUS_OFFLINE_TIMEOUT_MS 3000 // 3000 +#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK 0 // 0 +#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING 1 // 1 +#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR 2 // 2 +#define UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL 3 // 3 +#define UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL 0 // 0 +#define UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION 1 // 1 +#define UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE 2 // 2 +#define UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE 3 // 3 +#define UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE 7 // 7 + +typedef struct +{ + // FieldTypes + uint32_t uptime_sec; // bit len 32 + uint8_t health; // bit len 2 + uint8_t mode; // bit len 3 + uint8_t sub_mode; // bit len 3 + uint16_t vendor_specific_status_code; // bit len 16 + +} uavcan_protocol_NodeStatus; + +extern +uint32_t uavcan_protocol_NodeStatus_encode(uavcan_protocol_NodeStatus* source, void* msg_buf); + +extern +int32_t uavcan_protocol_NodeStatus_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_NodeStatus* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_NodeStatus_encode_internal(uavcan_protocol_NodeStatus* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_NodeStatus_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_NodeStatus* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_NODESTATUS \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/RestartNode.h b/libcanard/dsdl/uavcan/protocol/RestartNode.h new file mode 100644 index 00000000..97898216 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/RestartNode.h @@ -0,0 +1,98 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/5.RestartNode.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_RESTARTNODE +#define __UAVCAN_PROTOCOL_RESTARTNODE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Restart the node. +# +# Some nodes may require restart before the new configuration will be applied. +# +# The request should be rejected if magic_number does not equal MAGIC_NUMBER. +# + +uint40 MAGIC_NUMBER = 0xACCE551B1E +uint40 magic_number + +--- + +bool ok +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.RestartNode +saturated uint40 magic_number +--- +saturated bool ok +******************************************************************************/ + +#define UAVCAN_PROTOCOL_RESTARTNODE_ID 5 +#define UAVCAN_PROTOCOL_RESTARTNODE_NAME "uavcan.protocol.RestartNode" +#define UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE (0x569E05394A3017F0ULL) + +#define UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAX_SIZE ((40 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER 0xACCE551B1E // 0xACCE551B1E + +typedef struct +{ + // FieldTypes + uint64_t magic_number; // bit len 40 + +} uavcan_protocol_RestartNodeRequest; + +extern +uint32_t uavcan_protocol_RestartNodeRequest_encode(uavcan_protocol_RestartNodeRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_RestartNodeRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_RestartNodeRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_RestartNodeRequest_encode_internal(uavcan_protocol_RestartNodeRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_RestartNodeRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_RestartNodeRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_MAX_SIZE ((1 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + bool ok; // bit len 1 + +} uavcan_protocol_RestartNodeResponse; + +extern +uint32_t uavcan_protocol_RestartNodeResponse_encode(uavcan_protocol_RestartNodeResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_RestartNodeResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_RestartNodeResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_RestartNodeResponse_encode_internal(uavcan_protocol_RestartNodeResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_RestartNodeResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_RestartNodeResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_RESTARTNODE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/SoftwareVersion.h b/libcanard/dsdl/uavcan/protocol/SoftwareVersion.h new file mode 100644 index 00000000..888ddf5d --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/SoftwareVersion.h @@ -0,0 +1,111 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/SoftwareVersion.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_SOFTWAREVERSION +#define __UAVCAN_PROTOCOL_SOFTWAREVERSION + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Nested type. +# Generic software version information. +# + +# +# Primary version numbers. +# If both fields are set to zero, the version is considered unknown. +# +uint8 major +uint8 minor + +# +# This mask indicates which optional fields (see below) are set. +# +uint8 OPTIONAL_FIELD_FLAG_VCS_COMMIT = 1 +uint8 OPTIONAL_FIELD_FLAG_IMAGE_CRC = 2 +uint8 optional_field_flags + +# +# VCS commit hash or revision number, e.g. git short commit hash. Optional. +# +uint32 vcs_commit + +# +# The value of an arbitrary hash function applied to the firmware image. +# This field is used to detect whether the firmware running on the node is EXACTLY THE SAME +# as a certain specific revision. This field provides the absolute identity guarantee, unlike +# the version fields above, which can be the same for different builds of the firmware. +# +# The exact hash function and the methods of its application are implementation defined. +# However, implementations are recommended to adhere to the following guidelines, +# fully or partially: +# +# - The hash function should be CRC-64-WE, the same that is used for computing DSDL signatures. +# +# - The hash function should be applied to the entire application image padded to 8 bytes. +# +# - If the computed image CRC is stored within the firmware image itself, the value of +# the hash function becomes ill-defined, because it becomes recursively dependent on itself. +# In order to circumvent this issue, while computing or checking the CRC, its value stored +# within the image should be zeroed out. +# +uint64 image_crc +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.SoftwareVersion +saturated uint8 major +saturated uint8 minor +saturated uint8 optional_field_flags +saturated uint32 vcs_commit +saturated uint64 image_crc +******************************************************************************/ + +#define UAVCAN_PROTOCOL_SOFTWAREVERSION_NAME "uavcan.protocol.SoftwareVersion" +#define UAVCAN_PROTOCOL_SOFTWAREVERSION_SIGNATURE (0xDD46FD376527FEA1ULL) + +#define UAVCAN_PROTOCOL_SOFTWAREVERSION_MAX_SIZE ((120 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT 1 // 1 +#define UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_IMAGE_CRC 2 // 2 + +typedef struct +{ + // FieldTypes + uint8_t major; // bit len 8 + uint8_t minor; // bit len 8 + uint8_t optional_field_flags; // bit len 8 + uint32_t vcs_commit; // bit len 32 + uint64_t image_crc; // bit len 64 + +} uavcan_protocol_SoftwareVersion; + +extern +uint32_t uavcan_protocol_SoftwareVersion_encode(uavcan_protocol_SoftwareVersion* source, void* msg_buf); + +extern +int32_t uavcan_protocol_SoftwareVersion_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_SoftwareVersion* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_SoftwareVersion_encode_internal(uavcan_protocol_SoftwareVersion* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_SoftwareVersion_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_SoftwareVersion* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_SOFTWAREVERSION \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/BeginFirmwareUpdate.h b/libcanard/dsdl/uavcan/protocol/file/BeginFirmwareUpdate.h new file mode 100644 index 00000000..06919548 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/BeginFirmwareUpdate.h @@ -0,0 +1,130 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/40.BeginFirmwareUpdate.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE +#define __UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/******************************* Source text ********************************** +# +# This service initiates firmware update on a remote node. +# +# The node that is being updated (slave) will retrieve the firmware image file 'image_file_remote_path' from the node +# 'source_node_id' using the file read service, then it will update the firmware and reboot. +# +# The slave can explicitly reject this request if it is not possible to update the firmware at the moment +# (e.g. if the node is busy). +# +# If the slave node accepts this request, the initiator will get a response immediately, before the update process +# actually begins. +# +# While the firmware is being updated, the slave should set its mode (uavcan.protocol.NodeStatus.mode) to +# MODE_SOFTWARE_UPDATE. +# + +uint8 source_node_id # If this field is zero, the caller's Node ID will be used instead. +Path image_file_remote_path + +--- + +# +# Other error codes may be added in the future. +# +uint8 ERROR_OK = 0 +uint8 ERROR_INVALID_MODE = 1 # Cannot perform the update in the current operating mode or state. +uint8 ERROR_IN_PROGRESS = 2 # Firmware update is already in progress, and the slave doesn't want to restart. +uint8 ERROR_UNKNOWN = 255 +uint8 error + +uint8[<128] optional_error_message # Detailed description of the error. +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.BeginFirmwareUpdate +saturated uint8 source_node_id +uavcan.protocol.file.Path image_file_remote_path +--- +saturated uint8 error +saturated uint8[<=127] optional_error_message +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID 40 +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_NAME "uavcan.protocol.file.BeginFirmwareUpdate" +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE (0xB7D725DF72724126ULL) + +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_REQUEST_MAX_SIZE ((1616 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uint8_t source_node_id; // bit len 8 + uavcan_protocol_file_Path image_file_remote_path; // + +} uavcan_protocol_file_BeginFirmwareUpdateRequest; + +extern +uint32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_encode(uavcan_protocol_file_BeginFirmwareUpdateRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_BeginFirmwareUpdateRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_encode_internal(uavcan_protocol_file_BeginFirmwareUpdateRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_BeginFirmwareUpdateRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE ((1031 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK 0 // 0 +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_INVALID_MODE 1 // 1 +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_IN_PROGRESS 2 // 2 +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_UNKNOWN 255 // 255 + +#define UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_OPTIONAL_ERROR_MESSAGE_MAX_LENGTH 127 + +typedef struct +{ + // FieldTypes + uint8_t error; // bit len 8 + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[127] max items + } optional_error_message; + +} uavcan_protocol_file_BeginFirmwareUpdateResponse; + +extern +uint32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(uavcan_protocol_file_BeginFirmwareUpdateResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_BeginFirmwareUpdateResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_encode_internal(uavcan_protocol_file_BeginFirmwareUpdateResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_BeginFirmwareUpdateResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/Delete.h b/libcanard/dsdl/uavcan/protocol/file/Delete.h new file mode 100644 index 00000000..7d817cac --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/Delete.h @@ -0,0 +1,96 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/47.Delete.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_DELETE +#define __UAVCAN_PROTOCOL_FILE_DELETE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +/******************************* Source text ********************************** +# +# Delete remote file system entry. +# If the remote entry is a directory, all nested entries will be removed too. +# + +Path path + +--- + +Error error +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.Delete +uavcan.protocol.file.Path path +--- +uavcan.protocol.file.Error error +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_DELETE_ID 47 +#define UAVCAN_PROTOCOL_FILE_DELETE_NAME "uavcan.protocol.file.Delete" +#define UAVCAN_PROTOCOL_FILE_DELETE_SIGNATURE (0x78648C99170B47AAULL) + +#define UAVCAN_PROTOCOL_FILE_DELETE_REQUEST_MAX_SIZE ((1608 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uavcan_protocol_file_Path path; // + +} uavcan_protocol_file_DeleteRequest; + +extern +uint32_t uavcan_protocol_file_DeleteRequest_encode(uavcan_protocol_file_DeleteRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_DeleteRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_DeleteRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_DeleteRequest_encode_internal(uavcan_protocol_file_DeleteRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_DeleteRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_DeleteRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_FILE_DELETE_RESPONSE_MAX_SIZE ((16 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uavcan_protocol_file_Error error; // + +} uavcan_protocol_file_DeleteResponse; + +extern +uint32_t uavcan_protocol_file_DeleteResponse_encode(uavcan_protocol_file_DeleteResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_DeleteResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_DeleteResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_DeleteResponse_encode_internal(uavcan_protocol_file_DeleteResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_DeleteResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_DeleteResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_DELETE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/EntryType.h b/libcanard/dsdl/uavcan/protocol/file/EntryType.h new file mode 100644 index 00000000..51483a19 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/EntryType.h @@ -0,0 +1,75 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/EntryType.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_ENTRYTYPE +#define __UAVCAN_PROTOCOL_FILE_ENTRYTYPE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Nested type. +# Represents the type of the file system entry (e.g. file or directory). +# If such entry does not exist, 'flags' must be set to zero. +# + +uint8 FLAG_FILE = 1 # Excludes FLAG_DIRECTORY +uint8 FLAG_DIRECTORY = 2 # Excludes FLAG_FILE +uint8 FLAG_SYMLINK = 4 # Link target is either FLAG_FILE or FLAG_DIRECTORY +uint8 FLAG_READABLE = 8 +uint8 FLAG_WRITEABLE = 16 + +uint8 flags +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.EntryType +saturated uint8 flags +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_NAME "uavcan.protocol.file.EntryType" +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_SIGNATURE (0x6924572FBB2086E5ULL) + +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_MAX_SIZE ((8 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_FLAG_FILE 1 // 1 +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_FLAG_DIRECTORY 2 // 2 +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_FLAG_SYMLINK 4 // 4 +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_FLAG_READABLE 8 // 8 +#define UAVCAN_PROTOCOL_FILE_ENTRYTYPE_FLAG_WRITEABLE 16 // 16 + +typedef struct +{ + // FieldTypes + uint8_t flags; // bit len 8 + +} uavcan_protocol_file_EntryType; + +extern +uint32_t uavcan_protocol_file_EntryType_encode(uavcan_protocol_file_EntryType* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_EntryType_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_EntryType* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_EntryType_encode_internal(uavcan_protocol_file_EntryType* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_EntryType_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_EntryType* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_ENTRYTYPE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/Error.h b/libcanard/dsdl/uavcan/protocol/file/Error.h new file mode 100644 index 00000000..dc1dece0 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/Error.h @@ -0,0 +1,85 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/Error.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_ERROR +#define __UAVCAN_PROTOCOL_FILE_ERROR + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Nested type. +# File operation result code. +# + +int16 OK = 0 +int16 UNKNOWN_ERROR = 32767 + +int16 NOT_FOUND = 2 +int16 IO_ERROR = 5 +int16 ACCESS_DENIED = 13 +int16 IS_DIRECTORY = 21 # I.e. attempt to read/write on a path that points to a directory +int16 INVALID_VALUE = 22 # E.g. file name is not valid for the target file system +int16 FILE_TOO_LARGE = 27 +int16 OUT_OF_SPACE = 28 +int16 NOT_IMPLEMENTED = 38 + +int16 value +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.Error +saturated int16 value +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_ERROR_NAME "uavcan.protocol.file.Error" +#define UAVCAN_PROTOCOL_FILE_ERROR_SIGNATURE (0xA83071FFEA4FAE15ULL) + +#define UAVCAN_PROTOCOL_FILE_ERROR_MAX_SIZE ((16 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_FILE_ERROR_OK 0 // 0 +#define UAVCAN_PROTOCOL_FILE_ERROR_UNKNOWN_ERROR 32767 // 32767 +#define UAVCAN_PROTOCOL_FILE_ERROR_NOT_FOUND 2 // 2 +#define UAVCAN_PROTOCOL_FILE_ERROR_IO_ERROR 5 // 5 +#define UAVCAN_PROTOCOL_FILE_ERROR_ACCESS_DENIED 13 // 13 +#define UAVCAN_PROTOCOL_FILE_ERROR_IS_DIRECTORY 21 // 21 +#define UAVCAN_PROTOCOL_FILE_ERROR_INVALID_VALUE 22 // 22 +#define UAVCAN_PROTOCOL_FILE_ERROR_FILE_TOO_LARGE 27 // 27 +#define UAVCAN_PROTOCOL_FILE_ERROR_OUT_OF_SPACE 28 // 28 +#define UAVCAN_PROTOCOL_FILE_ERROR_NOT_IMPLEMENTED 38 // 38 + +typedef struct +{ + // FieldTypes + int16_t value; // bit len 16 + +} uavcan_protocol_file_Error; + +extern +uint32_t uavcan_protocol_file_Error_encode(uavcan_protocol_file_Error* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_Error_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_Error* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_Error_encode_internal(uavcan_protocol_file_Error* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_Error_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_Error* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_ERROR \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/GetDirectoryEntryInfo.h b/libcanard/dsdl/uavcan/protocol/file/GetDirectoryEntryInfo.h new file mode 100644 index 00000000..799eba9a --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/GetDirectoryEntryInfo.h @@ -0,0 +1,114 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/46.GetDirectoryEntryInfo.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO +#define __UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +/******************************* Source text ********************************** +# +# This service can be used to retrieve a remote directory listing, one entry per request. +# +# The client should query each entry independently, iterating 'entry_index' from 0 until the last entry is passed, +# in which case the server will report that there is no such entry (via the fields 'entry_type' and 'error'). +# +# The entry_index shall be applied to the ordered list of directory entries (e.g. alphabetically ordered). The exact +# sorting criteria does not matter as long as it provides the same ordering for subsequent service calls. +# + +uint32 entry_index + +Path directory_path + +--- + +Error error + +EntryType entry_type + +Path entry_full_path # Ignored/Empty if such entry does not exist. +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.GetDirectoryEntryInfo +saturated uint32 entry_index +uavcan.protocol.file.Path directory_path +--- +uavcan.protocol.file.Error error +uavcan.protocol.file.EntryType entry_type +uavcan.protocol.file.Path entry_full_path +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO_ID 46 +#define UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO_NAME "uavcan.protocol.file.GetDirectoryEntryInfo" +#define UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO_SIGNATURE (0x8C46E8AB568BDA79ULL) + +#define UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO_REQUEST_MAX_SIZE ((1640 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uint32_t entry_index; // bit len 32 + uavcan_protocol_file_Path directory_path; // + +} uavcan_protocol_file_GetDirectoryEntryInfoRequest; + +extern +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_encode(uavcan_protocol_file_GetDirectoryEntryInfoRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetDirectoryEntryInfoRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_encode_internal(uavcan_protocol_file_GetDirectoryEntryInfoRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetDirectoryEntryInfoRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO_RESPONSE_MAX_SIZE ((1632 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uavcan_protocol_file_Error error; // + uavcan_protocol_file_EntryType entry_type; // + uavcan_protocol_file_Path entry_full_path; // + +} uavcan_protocol_file_GetDirectoryEntryInfoResponse; + +extern +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_encode(uavcan_protocol_file_GetDirectoryEntryInfoResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetDirectoryEntryInfoResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_encode_internal(uavcan_protocol_file_GetDirectoryEntryInfoResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetDirectoryEntryInfoResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_GETDIRECTORYENTRYINFO \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/GetInfo.h b/libcanard/dsdl/uavcan/protocol/file/GetInfo.h new file mode 100644 index 00000000..dc3f9266 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/GetInfo.h @@ -0,0 +1,108 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/45.GetInfo.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_GETINFO +#define __UAVCAN_PROTOCOL_FILE_GETINFO + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +/******************************* Source text ********************************** +# +# Request info about a remote file system entry (file, directory, etc). +# + +Path path + +--- + +# +# File size in bytes. +# Should be set to zero for directories. +# +uint40 size + +Error error + +EntryType entry_type +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.GetInfo +uavcan.protocol.file.Path path +--- +saturated uint40 size +uavcan.protocol.file.Error error +uavcan.protocol.file.EntryType entry_type +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_GETINFO_ID 45 +#define UAVCAN_PROTOCOL_FILE_GETINFO_NAME "uavcan.protocol.file.GetInfo" +#define UAVCAN_PROTOCOL_FILE_GETINFO_SIGNATURE (0x5004891EE8A27531ULL) + +#define UAVCAN_PROTOCOL_FILE_GETINFO_REQUEST_MAX_SIZE ((1608 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uavcan_protocol_file_Path path; // + +} uavcan_protocol_file_GetInfoRequest; + +extern +uint32_t uavcan_protocol_file_GetInfoRequest_encode(uavcan_protocol_file_GetInfoRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_GetInfoRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetInfoRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_GetInfoRequest_encode_internal(uavcan_protocol_file_GetInfoRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_GetInfoRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetInfoRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_FILE_GETINFO_RESPONSE_MAX_SIZE ((64 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uint64_t size; // bit len 40 + uavcan_protocol_file_Error error; // + uavcan_protocol_file_EntryType entry_type; // + +} uavcan_protocol_file_GetInfoResponse; + +extern +uint32_t uavcan_protocol_file_GetInfoResponse_encode(uavcan_protocol_file_GetInfoResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_GetInfoResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetInfoResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_GetInfoResponse_encode_internal(uavcan_protocol_file_GetInfoResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_GetInfoResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_GetInfoResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_GETINFO \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/Path.h b/libcanard/dsdl/uavcan/protocol/file/Path.h new file mode 100644 index 00000000..93db0b91 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/Path.h @@ -0,0 +1,75 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/Path.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_PATH +#define __UAVCAN_PROTOCOL_FILE_PATH + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Nested type. +# +# File system path in UTF8. +# +# The only valid separator is forward slash. +# + +uint8 SEPARATOR = '/' + +uint8[<=200] path +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.Path +saturated uint8[<=200] path +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_PATH_NAME "uavcan.protocol.file.Path" +#define UAVCAN_PROTOCOL_FILE_PATH_SIGNATURE (0x12AEFC50878A43E2ULL) + +#define UAVCAN_PROTOCOL_FILE_PATH_MAX_SIZE ((1608 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_FILE_PATH_SEPARATOR '/' // '/' + +#define UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH 200 + +typedef struct +{ + // FieldTypes + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[200] max items + } path; + +} uavcan_protocol_file_Path; + +extern +uint32_t uavcan_protocol_file_Path_encode(uavcan_protocol_file_Path* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_Path_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_Path* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_Path_encode_internal(uavcan_protocol_file_Path* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_Path_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_Path* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_PATH \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/Read.h b/libcanard/dsdl/uavcan/protocol/file/Read.h new file mode 100644 index 00000000..f22376c3 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/Read.h @@ -0,0 +1,119 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/48.Read.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_READ +#define __UAVCAN_PROTOCOL_FILE_READ + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +/******************************* Source text ********************************** +# +# Read file from a remote node. +# +# There are two possible outcomes of a successful service call: +# 1. Data array size equals its capacity. This means that the end of the file is not reached yet. +# 2. Data array size is less than its capacity, possibly zero. This means that the end of file is reached. +# +# Thus, if the client needs to fetch the entire file, it should repeatedly call this service while increasing the +# offset, until incomplete data is returned. +# +# If the object pointed by 'path' cannot be read (e.g. it is a directory or it does not exist), appropriate error code +# will be returned, and data array will be empty. +# + +uint40 offset + +Path path + +--- + +Error error + +uint8[<=256] data +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.Read +saturated uint40 offset +uavcan.protocol.file.Path path +--- +uavcan.protocol.file.Error error +saturated uint8[<=256] data +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_READ_ID 48 +#define UAVCAN_PROTOCOL_FILE_READ_NAME "uavcan.protocol.file.Read" +#define UAVCAN_PROTOCOL_FILE_READ_SIGNATURE (0x8DCDCA939F33F678ULL) + +#define UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE ((1648 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uint64_t offset; // bit len 40 + uavcan_protocol_file_Path path; // + +} uavcan_protocol_file_ReadRequest; + +extern +uint32_t uavcan_protocol_file_ReadRequest_encode(uavcan_protocol_file_ReadRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_ReadRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_ReadRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_ReadRequest_encode_internal(uavcan_protocol_file_ReadRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_ReadRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_ReadRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_FILE_READ_RESPONSE_MAX_SIZE ((2073 + 7)/8) + +// Constants + +#define UAVCAN_PROTOCOL_FILE_READ_RESPONSE_DATA_MAX_LENGTH 256 + +typedef struct +{ + // FieldTypes + uavcan_protocol_file_Error error; // + struct + { + uint16_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[256] max items + } data; + +} uavcan_protocol_file_ReadResponse; + +extern +uint32_t uavcan_protocol_file_ReadResponse_encode(uavcan_protocol_file_ReadResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_ReadResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_ReadResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_ReadResponse_encode_internal(uavcan_protocol_file_ReadResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_ReadResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_ReadResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_READ \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/Write.h b/libcanard/dsdl/uavcan/protocol/file/Write.h new file mode 100644 index 00000000..0a5fef99 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/Write.h @@ -0,0 +1,122 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/49.Write.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_FILE_WRITE +#define __UAVCAN_PROTOCOL_FILE_WRITE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +/******************************* Source text ********************************** +# +# Write into a remote file. +# The server shall place the contents of the field 'data' into the file pointed by 'path' at the offset specified by +# the field 'offset'. +# +# When writing a file, the client should repeatedly call this service with data while advancing offset until the file +# is written completely. When write is complete, the client shall call the service one last time, with the offset +# set to the size of the file and with the data field empty, which will signal the server that the write operation is +# complete. +# +# When the write operation is complete, the server shall truncate the resulting file past the specified offset. +# +# Server implementation advice: +# It is recommended to implement proper handling of concurrent writes to the same file from different clients, for +# example by means of creating a staging area for uncompleted writes (like FTP servers do). +# + +uint40 offset + +Path path + +uint8[<=192] data + +--- + +Error error +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.file.Write +saturated uint40 offset +uavcan.protocol.file.Path path +saturated uint8[<=192] data +--- +uavcan.protocol.file.Error error +******************************************************************************/ + +#define UAVCAN_PROTOCOL_FILE_WRITE_ID 49 +#define UAVCAN_PROTOCOL_FILE_WRITE_NAME "uavcan.protocol.file.Write" +#define UAVCAN_PROTOCOL_FILE_WRITE_SIGNATURE (0x515AA1DC77E58429ULL) + +#define UAVCAN_PROTOCOL_FILE_WRITE_REQUEST_MAX_SIZE ((3192 + 7)/8) + +// Constants + +#define UAVCAN_PROTOCOL_FILE_WRITE_REQUEST_DATA_MAX_LENGTH 192 + +typedef struct +{ + // FieldTypes + uint64_t offset; // bit len 40 + uavcan_protocol_file_Path path; // + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[192] max items + } data; + +} uavcan_protocol_file_WriteRequest; + +extern +uint32_t uavcan_protocol_file_WriteRequest_encode(uavcan_protocol_file_WriteRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_WriteRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_WriteRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_WriteRequest_encode_internal(uavcan_protocol_file_WriteRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_WriteRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_WriteRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_FILE_WRITE_RESPONSE_MAX_SIZE ((16 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + uavcan_protocol_file_Error error; // + +} uavcan_protocol_file_WriteResponse; + +extern +uint32_t uavcan_protocol_file_WriteResponse_encode(uavcan_protocol_file_WriteResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_file_WriteResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_WriteResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_file_WriteResponse_encode_internal(uavcan_protocol_file_WriteResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_file_WriteResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_file_WriteResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_FILE_WRITE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/file/file_BeginFirmwareUpdate.c b/libcanard/dsdl/uavcan/protocol/file/file_BeginFirmwareUpdate.c new file mode 100644 index 00000000..6f64b33e --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_BeginFirmwareUpdate.c @@ -0,0 +1,305 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/40.BeginFirmwareUpdate.uavcan + */ +#include "uavcan/protocol/file/BeginFirmwareUpdate.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_encode_internal(uavcan_protocol_file_BeginFirmwareUpdateRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->source_node_id); // 255 + offset += 8; + + // Compound + offset = uavcan_protocol_file_Path_encode_internal(&source->image_file_remote_path, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_encode(uavcan_protocol_file_BeginFirmwareUpdateRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_BeginFirmwareUpdateRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_BeginFirmwareUpdateRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_BeginFirmwareUpdateRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->source_node_id); + if (ret != 8) + { + goto uavcan_protocol_file_BeginFirmwareUpdateRequest_error_exit; + } + offset += 8; + + // Compound + offset = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, &dest->image_file_remote_path, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_BeginFirmwareUpdateRequest_error_exit; + } + return offset; + +uavcan_protocol_file_BeginFirmwareUpdateRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_BeginFirmwareUpdateRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_BeginFirmwareUpdateRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_BeginFirmwareUpdateRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_BeginFirmwareUpdateRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_encode_internal(uavcan_protocol_file_BeginFirmwareUpdateResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->error); // 255 + offset += 8; + + // Dynamic Array (optional_error_message) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 7, (void*)&source->optional_error_message.len); + offset += 7; + } + + // - Add array items + for (c = 0; c < source->optional_error_message.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->optional_error_message.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(uavcan_protocol_file_BeginFirmwareUpdateResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_BeginFirmwareUpdateResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_BeginFirmwareUpdateResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->error); + if (ret != 8) + { + goto uavcan_protocol_file_BeginFirmwareUpdateResponse_error_exit; + } + offset += 8; + + // Dynamic Array (optional_error_message) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->optional_error_message.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 7 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 7, + false, + (void*)&dest->optional_error_message.len); // 255 + if (ret != 7) + { + goto uavcan_protocol_file_BeginFirmwareUpdateResponse_error_exit; + } + offset += 7; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->optional_error_message.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->optional_error_message.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_file_BeginFirmwareUpdateResponse_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_file_BeginFirmwareUpdateResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_BeginFirmwareUpdateResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_BeginFirmwareUpdateResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_BeginFirmwareUpdateResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_BeginFirmwareUpdateResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_BeginFirmwareUpdateResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_BeginFirmwareUpdateResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_Delete.c b/libcanard/dsdl/uavcan/protocol/file/file_Delete.c new file mode 100644 index 00000000..8ceec082 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_Delete.c @@ -0,0 +1,229 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/47.Delete.uavcan + */ +#include "uavcan/protocol/file/Delete.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_DeleteRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_DeleteRequest_encode_internal(uavcan_protocol_file_DeleteRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + // Compound + offset = uavcan_protocol_file_Path_encode_internal(&source->path, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_DeleteRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_DeleteRequest_encode(uavcan_protocol_file_DeleteRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_DeleteRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_DeleteRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_DeleteRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_DeleteRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_DeleteRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + // Compound + offset = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, &dest->path, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_DeleteRequest_error_exit; + } + return offset; + +uavcan_protocol_file_DeleteRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_DeleteRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_DeleteRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_DeleteRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_DeleteRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_DeleteRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_DeleteRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_file_DeleteResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_DeleteResponse_encode_internal(uavcan_protocol_file_DeleteResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + // Compound + offset = uavcan_protocol_file_Error_encode_internal(&source->error, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_DeleteResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_DeleteResponse_encode(uavcan_protocol_file_DeleteResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_DeleteResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_DeleteResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_DeleteResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_DeleteResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_DeleteResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + // Compound + offset = uavcan_protocol_file_Error_decode_internal(transfer, payload_len, &dest->error, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_DeleteResponse_error_exit; + } + return offset; + +uavcan_protocol_file_DeleteResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_DeleteResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_DeleteResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_DeleteResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_DeleteResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_DeleteResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_DeleteResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_EntryType.c b/libcanard/dsdl/uavcan/protocol/file/file_EntryType.c new file mode 100644 index 00000000..96b3c73a --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_EntryType.c @@ -0,0 +1,125 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/EntryType.uavcan + */ +#include "uavcan/protocol/file/EntryType.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_EntryType_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_EntryType_encode_internal(uavcan_protocol_file_EntryType* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->flags); // 255 + offset += 8; + + return offset; +} + +/** + * @brief uavcan_protocol_file_EntryType_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_EntryType_encode(uavcan_protocol_file_EntryType* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_EntryType_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_EntryType_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_EntryType dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_EntryType_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_EntryType* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->flags); + if (ret != 8) + { + goto uavcan_protocol_file_EntryType_error_exit; + } + offset += 8; + return offset; + +uavcan_protocol_file_EntryType_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_EntryType_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_EntryType dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_EntryType_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_EntryType* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_EntryType); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_EntryType_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_Error.c b/libcanard/dsdl/uavcan/protocol/file/file_Error.c new file mode 100644 index 00000000..1af8896a --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_Error.c @@ -0,0 +1,125 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/Error.uavcan + */ +#include "uavcan/protocol/file/Error.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_Error_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_Error_encode_internal(uavcan_protocol_file_Error* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + canardEncodeScalar(msg_buf, offset, 16, (void*)&source->value); // 32767 + offset += 16; + + return offset; +} + +/** + * @brief uavcan_protocol_file_Error_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_Error_encode(uavcan_protocol_file_Error* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_Error_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_Error_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_Error dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_Error_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_Error* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, true, (void*)&dest->value); + if (ret != 16) + { + goto uavcan_protocol_file_Error_error_exit; + } + offset += 16; + return offset; + +uavcan_protocol_file_Error_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_Error_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_Error dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_Error_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_Error* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_Error); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_Error_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_GetDirectoryEntryInfo.c b/libcanard/dsdl/uavcan/protocol/file/file_GetDirectoryEntryInfo.c new file mode 100644 index 00000000..efdfe45b --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_GetDirectoryEntryInfo.c @@ -0,0 +1,261 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/46.GetDirectoryEntryInfo.uavcan + */ +#include "uavcan/protocol/file/GetDirectoryEntryInfo.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_encode_internal(uavcan_protocol_file_GetDirectoryEntryInfoRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + canardEncodeScalar(msg_buf, offset, 32, (void*)&source->entry_index); // 4294967295 + offset += 32; + + // Compound + offset = uavcan_protocol_file_Path_encode_internal(&source->directory_path, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_encode(uavcan_protocol_file_GetDirectoryEntryInfoRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_GetDirectoryEntryInfoRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetDirectoryEntryInfoRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_GetDirectoryEntryInfoRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 32, false, (void*)&dest->entry_index); + if (ret != 32) + { + goto uavcan_protocol_file_GetDirectoryEntryInfoRequest_error_exit; + } + offset += 32; + + // Compound + offset = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, &dest->directory_path, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_GetDirectoryEntryInfoRequest_error_exit; + } + return offset; + +uavcan_protocol_file_GetDirectoryEntryInfoRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetDirectoryEntryInfoRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetDirectoryEntryInfoRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_GetDirectoryEntryInfoRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_GetDirectoryEntryInfoRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_GetDirectoryEntryInfoRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_encode_internal(uavcan_protocol_file_GetDirectoryEntryInfoResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + // Compound + offset = uavcan_protocol_file_Error_encode_internal(&source->error, msg_buf, offset, 0); + + // Compound + offset = uavcan_protocol_file_EntryType_encode_internal(&source->entry_type, msg_buf, offset, 0); + + // Compound + offset = uavcan_protocol_file_Path_encode_internal(&source->entry_full_path, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_encode(uavcan_protocol_file_GetDirectoryEntryInfoResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_GetDirectoryEntryInfoResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetDirectoryEntryInfoResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_GetDirectoryEntryInfoResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + // Compound + offset = uavcan_protocol_file_Error_decode_internal(transfer, payload_len, &dest->error, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_GetDirectoryEntryInfoResponse_error_exit; + } + + // Compound + offset = uavcan_protocol_file_EntryType_decode_internal(transfer, payload_len, &dest->entry_type, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_GetDirectoryEntryInfoResponse_error_exit; + } + + // Compound + offset = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, &dest->entry_full_path, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_GetDirectoryEntryInfoResponse_error_exit; + } + return offset; + +uavcan_protocol_file_GetDirectoryEntryInfoResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_GetDirectoryEntryInfoResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetDirectoryEntryInfoResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetDirectoryEntryInfoResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_GetDirectoryEntryInfoResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_GetDirectoryEntryInfoResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_GetDirectoryEntryInfoResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_GetInfo.c b/libcanard/dsdl/uavcan/protocol/file/file_GetInfo.c new file mode 100644 index 00000000..6a9ce5c6 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_GetInfo.c @@ -0,0 +1,251 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/45.GetInfo.uavcan + */ +#include "uavcan/protocol/file/GetInfo.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_GetInfoRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_GetInfoRequest_encode_internal(uavcan_protocol_file_GetInfoRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + // Compound + offset = uavcan_protocol_file_Path_encode_internal(&source->path, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_GetInfoRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_GetInfoRequest_encode(uavcan_protocol_file_GetInfoRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_GetInfoRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_GetInfoRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetInfoRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetInfoRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_GetInfoRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + // Compound + offset = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, &dest->path, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_GetInfoRequest_error_exit; + } + return offset; + +uavcan_protocol_file_GetInfoRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_GetInfoRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetInfoRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetInfoRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_GetInfoRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_GetInfoRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_GetInfoRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_file_GetInfoResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_GetInfoResponse_encode_internal(uavcan_protocol_file_GetInfoResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + source->size = CANARD_INTERNAL_SATURATE_UNSIGNED(source->size, 1099511627775) + canardEncodeScalar(msg_buf, offset, 40, (void*)&source->size); // 1099511627775 + offset += 40; + + // Compound + offset = uavcan_protocol_file_Error_encode_internal(&source->error, msg_buf, offset, 0); + + // Compound + offset = uavcan_protocol_file_EntryType_encode_internal(&source->entry_type, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_GetInfoResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_GetInfoResponse_encode(uavcan_protocol_file_GetInfoResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_GetInfoResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_GetInfoResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetInfoResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetInfoResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_GetInfoResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 40, false, (void*)&dest->size); + if (ret != 40) + { + goto uavcan_protocol_file_GetInfoResponse_error_exit; + } + offset += 40; + + // Compound + offset = uavcan_protocol_file_Error_decode_internal(transfer, payload_len, &dest->error, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_GetInfoResponse_error_exit; + } + + // Compound + offset = uavcan_protocol_file_EntryType_decode_internal(transfer, payload_len, &dest->entry_type, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_GetInfoResponse_error_exit; + } + return offset; + +uavcan_protocol_file_GetInfoResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_GetInfoResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_GetInfoResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_GetInfoResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_GetInfoResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_GetInfoResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_GetInfoResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_Path.c b/libcanard/dsdl/uavcan/protocol/file/file_Path.c new file mode 100644 index 00000000..93c0f15f --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_Path.c @@ -0,0 +1,182 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/Path.uavcan + */ +#include "uavcan/protocol/file/Path.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_Path_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_Path_encode_internal(uavcan_protocol_file_Path* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + // Dynamic Array (path) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->path.len); + offset += 8; + } + + // - Add array items + for (c = 0; c < source->path.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->path.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_file_Path_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_Path_encode(uavcan_protocol_file_Path* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_Path_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_Path_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_Path dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_Path_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_Path* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + // Dynamic Array (path) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->path.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 8 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)&dest->path.len); // 255 + if (ret != 8) + { + goto uavcan_protocol_file_Path_error_exit; + } + offset += 8; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->path.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->path.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_file_Path_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_file_Path_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_Path_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_Path dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_Path_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_Path* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_Path); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_Read.c b/libcanard/dsdl/uavcan/protocol/file/file_Read.c new file mode 100644 index 00000000..be83cf67 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_Read.c @@ -0,0 +1,307 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/48.Read.uavcan + */ +#include "uavcan/protocol/file/Read.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_ReadRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_ReadRequest_encode_internal(uavcan_protocol_file_ReadRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + source->offset = CANARD_INTERNAL_SATURATE_UNSIGNED(source->offset, 1099511627775) + canardEncodeScalar(msg_buf, offset, 40, (void*)&source->offset); // 1099511627775 + offset += 40; + + // Compound + offset = uavcan_protocol_file_Path_encode_internal(&source->path, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_ReadRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_ReadRequest_encode(uavcan_protocol_file_ReadRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_ReadRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_ReadRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_ReadRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_ReadRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_ReadRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 40, false, (void*)&dest->offset); + if (ret != 40) + { + goto uavcan_protocol_file_ReadRequest_error_exit; + } + offset += 40; + + // Compound + offset = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, &dest->path, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_ReadRequest_error_exit; + } + return offset; + +uavcan_protocol_file_ReadRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_ReadRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_ReadRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_ReadRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_ReadRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_ReadRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_ReadRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_file_ReadResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_ReadResponse_encode_internal(uavcan_protocol_file_ReadResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + // Compound + offset = uavcan_protocol_file_Error_encode_internal(&source->error, msg_buf, offset, 0); + + // Dynamic Array (data) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 9, (void*)&source->data.len); + offset += 9; + } + + // - Add array items + for (c = 0; c < source->data.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->data.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_file_ReadResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_ReadResponse_encode(uavcan_protocol_file_ReadResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_ReadResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_ReadResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_ReadResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_ReadResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_ReadResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + // Compound + offset = uavcan_protocol_file_Error_decode_internal(transfer, payload_len, &dest->error, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_ReadResponse_error_exit; + } + + // Dynamic Array (data) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->data.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 9 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 9, + false, + (void*)&dest->data.len); // 255 + if (ret != 9) + { + goto uavcan_protocol_file_ReadResponse_error_exit; + } + offset += 9; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->data.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->data.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_file_ReadResponse_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_file_ReadResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_ReadResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_ReadResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_ReadResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_ReadResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_ReadResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_ReadResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/file/file_Write.c b/libcanard/dsdl/uavcan/protocol/file/file_Write.c new file mode 100644 index 00000000..ee79eb13 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/file/file_Write.c @@ -0,0 +1,307 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/file/49.Write.uavcan + */ +#include "uavcan/protocol/file/Write.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_file_WriteRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_WriteRequest_encode_internal(uavcan_protocol_file_WriteRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + source->offset = CANARD_INTERNAL_SATURATE_UNSIGNED(source->offset, 1099511627775) + canardEncodeScalar(msg_buf, offset, 40, (void*)&source->offset); // 1099511627775 + offset += 40; + + // Compound + offset = uavcan_protocol_file_Path_encode_internal(&source->path, msg_buf, offset, 0); + + // Dynamic Array (data) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->data.len); + offset += 8; + } + + // - Add array items + for (c = 0; c < source->data.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->data.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_file_WriteRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_WriteRequest_encode(uavcan_protocol_file_WriteRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_WriteRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_WriteRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_WriteRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_WriteRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_WriteRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 40, false, (void*)&dest->offset); + if (ret != 40) + { + goto uavcan_protocol_file_WriteRequest_error_exit; + } + offset += 40; + + // Compound + offset = uavcan_protocol_file_Path_decode_internal(transfer, payload_len, &dest->path, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_WriteRequest_error_exit; + } + + // Dynamic Array (data) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->data.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 8 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)&dest->data.len); // 255 + if (ret != 8) + { + goto uavcan_protocol_file_WriteRequest_error_exit; + } + offset += 8; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->data.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->data.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_file_WriteRequest_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_file_WriteRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_WriteRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_WriteRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_WriteRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_WriteRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_WriteRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_WriteRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_file_WriteResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_file_WriteResponse_encode_internal(uavcan_protocol_file_WriteResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + // Compound + offset = uavcan_protocol_file_Error_encode_internal(&source->error, msg_buf, offset, 0); + + return offset; +} + +/** + * @brief uavcan_protocol_file_WriteResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_file_WriteResponse_encode(uavcan_protocol_file_WriteResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_file_WriteResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_file_WriteResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_WriteResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_WriteResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_file_WriteResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + // Compound + offset = uavcan_protocol_file_Error_decode_internal(transfer, payload_len, &dest->error, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_file_WriteResponse_error_exit; + } + return offset; + +uavcan_protocol_file_WriteResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_file_WriteResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_file_WriteResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_file_WriteResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_file_WriteResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_file_WriteResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_file_WriteResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/param/Empty.h b/libcanard/dsdl/uavcan/protocol/param/Empty.h new file mode 100644 index 00000000..c5b8cbcd --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/Empty.h @@ -0,0 +1,55 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/Empty.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_PARAM_EMPTY +#define __UAVCAN_PROTOCOL_PARAM_EMPTY + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Ex nihilo nihil fit. +# +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.param.Empty +******************************************************************************/ + +#define UAVCAN_PROTOCOL_PARAM_EMPTY_NAME "uavcan.protocol.param.Empty" +#define UAVCAN_PROTOCOL_PARAM_EMPTY_SIGNATURE (0x6C4D0E8EF37361DFULL) + +#define UAVCAN_PROTOCOL_PARAM_EMPTY_MAX_SIZE ((0 + 7)/8) + +typedef struct +{ + uint8_t empty; +} uavcan_protocol_param_Empty; + +extern +uint32_t uavcan_protocol_param_Empty_encode(uavcan_protocol_param_Empty* source, void* msg_buf); + +extern +int32_t uavcan_protocol_param_Empty_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_Empty* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_param_Empty_encode_internal(uavcan_protocol_param_Empty* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_param_Empty_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_Empty* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_PARAM_EMPTY \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/param/ExecuteOpcode.h b/libcanard/dsdl/uavcan/protocol/param/ExecuteOpcode.h new file mode 100644 index 00000000..1f9ce5c6 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/ExecuteOpcode.h @@ -0,0 +1,123 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/10.ExecuteOpcode.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE +#define __UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# Service to control the node configuration. +# + +# +# SAVE operation instructs the remote node to save the current configuration parameters into a non-volatile +# storage. The node may require a restart in order for some changes to take effect. +# +# ERASE operation instructs the remote node to clear its configuration storage and reinitialize the parameters +# with their default values. The node may require a restart in order for some changes to take effect. +# +# Other opcodes may be added in the future (for example, an opcode for switching between multiple configurations). +# +uint8 OPCODE_SAVE = 0 # Save all parameters to non-volatile storage. +uint8 OPCODE_ERASE = 1 # Clear the non-volatile storage; some changes may take effect only after reboot. +uint8 opcode + +# +# Reserved, keep zero. +# +int48 argument + +--- + +# +# If 'ok' (the field below) is true, this value is not used and must be kept zero. +# If 'ok' is false, this value may contain error code. Error code constants may be defined in the future. +# +int48 argument + +# +# True if the operation has been performed successfully, false otherwise. +# +bool ok +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.param.ExecuteOpcode +saturated uint8 opcode +saturated int48 argument +--- +saturated int48 argument +saturated bool ok +******************************************************************************/ + +#define UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID 10 +#define UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_NAME "uavcan.protocol.param.ExecuteOpcode" +#define UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE (0x3B131AC5EB69D2CDULL) + +#define UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_MAX_SIZE ((56 + 7)/8) + +// Constants +#define UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE 0 // 0 +#define UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE 1 // 1 + +typedef struct +{ + // FieldTypes + uint8_t opcode; // bit len 8 + int64_t argument; // bit len 48 + +} uavcan_protocol_param_ExecuteOpcodeRequest; + +extern +uint32_t uavcan_protocol_param_ExecuteOpcodeRequest_encode(uavcan_protocol_param_ExecuteOpcodeRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_param_ExecuteOpcodeRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_ExecuteOpcodeRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_param_ExecuteOpcodeRequest_encode_internal(uavcan_protocol_param_ExecuteOpcodeRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_param_ExecuteOpcodeRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_ExecuteOpcodeRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE ((49 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + int64_t argument; // bit len 48 + bool ok; // bit len 1 + +} uavcan_protocol_param_ExecuteOpcodeResponse; + +extern +uint32_t uavcan_protocol_param_ExecuteOpcodeResponse_encode(uavcan_protocol_param_ExecuteOpcodeResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_param_ExecuteOpcodeResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_ExecuteOpcodeResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_param_ExecuteOpcodeResponse_encode_internal(uavcan_protocol_param_ExecuteOpcodeResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_param_ExecuteOpcodeResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_ExecuteOpcodeResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/param/GetSet.h b/libcanard/dsdl/uavcan/protocol/param/GetSet.h new file mode 100644 index 00000000..9f4650a5 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/GetSet.h @@ -0,0 +1,172 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/11.GetSet.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_PARAM_GETSET +#define __UAVCAN_PROTOCOL_PARAM_GETSET + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +/******************************* Source text ********************************** +# +# Get or set a parameter by name or by index. +# Note that access by index should only be used to retrieve the list of parameters; it is highly +# discouraged to use it for anything else, because persistent ordering is not guaranteed. +# + +# +# Index of the parameter starting from 0; ignored if name is nonempty. +# Use index only to retrieve the list of parameters. +# Parameter ordering must be well defined (e.g. alphabetical, or any other stable ordering), +# in order for the index access to work. +# +uint13 index + +# +# If set - parameter will be assigned this value, then the new value will be returned. +# If not set - current parameter value will be returned. +# Refer to the definition of Value for details. +# +Value value + +# +# Name of the parameter; always preferred over index if nonempty. +# +uint8[<=92] name + +--- + +void5 + +# +# Actual parameter value. +# +# For set requests, it should contain the actual parameter value after the set request was +# executed. The objective is to let the client know if the value could not be updated, e.g. +# due to its range violation, etc. +# +# Empty value (and/or empty name) indicates that there is no such parameter. +# +Value value + +void5 +Value default_value # Optional + +void6 +NumericValue max_value # Optional, not applicable for bool/string + +void6 +NumericValue min_value # Optional, not applicable for bool/string + +# +# Empty name (and/or empty value) in response indicates that there is no such parameter. +# +uint8[<=92] name +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.param.GetSet +saturated uint13 index +uavcan.protocol.param.Value value +saturated uint8[<=92] name +--- +void5 +uavcan.protocol.param.Value value +void5 +uavcan.protocol.param.Value default_value +void6 +uavcan.protocol.param.NumericValue max_value +void6 +uavcan.protocol.param.NumericValue min_value +saturated uint8[<=92] name +******************************************************************************/ + +#define UAVCAN_PROTOCOL_PARAM_GETSET_ID 11 +#define UAVCAN_PROTOCOL_PARAM_GETSET_NAME "uavcan.protocol.param.GetSet" +#define UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE (0xA7B622F939D1A4D5ULL) + +#define UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_MAX_SIZE ((1791 + 7)/8) + +// Constants + +#define UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_NAME_MAX_LENGTH 92 + +typedef struct +{ + // FieldTypes + uint16_t index; // bit len 13 + uavcan_protocol_param_Value value; // + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[92] max items + } name; + +} uavcan_protocol_param_GetSetRequest; + +extern +uint32_t uavcan_protocol_param_GetSetRequest_encode(uavcan_protocol_param_GetSetRequest* source, void* msg_buf); + +extern +int32_t uavcan_protocol_param_GetSetRequest_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_GetSetRequest* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_param_GetSetRequest_encode_internal(uavcan_protocol_param_GetSetRequest* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_param_GetSetRequest_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_GetSetRequest* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#define UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE ((2967 + 7)/8) + +// Constants + +#define UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_NAME_MAX_LENGTH 92 + +typedef struct +{ + // FieldTypes + // void5 + uavcan_protocol_param_Value value; // + // void5 + uavcan_protocol_param_Value default_value; // + // void6 + uavcan_protocol_param_NumericValue max_value; // + // void6 + uavcan_protocol_param_NumericValue min_value; // + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[92] max items + } name; + +} uavcan_protocol_param_GetSetResponse; + +extern +uint32_t uavcan_protocol_param_GetSetResponse_encode(uavcan_protocol_param_GetSetResponse* source, void* msg_buf); + +extern +int32_t uavcan_protocol_param_GetSetResponse_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_GetSetResponse* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_param_GetSetResponse_encode_internal(uavcan_protocol_param_GetSetResponse* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_param_GetSetResponse_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_GetSetResponse* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_PARAM_GETSET \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/param/NumericValue.h b/libcanard/dsdl/uavcan/protocol/param/NumericValue.h new file mode 100644 index 00000000..b6de0042 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/NumericValue.h @@ -0,0 +1,89 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/NumericValue.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_PARAM_NUMERICVALUE +#define __UAVCAN_PROTOCOL_PARAM_NUMERICVALUE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/******************************* Source text ********************************** +# +# Numeric-only value. +# +# This is a union, which means that this structure can contain either one of the fields below. +# The structure is prefixed with tag - a selector value that indicates which particular field is encoded. +# + +@union # Tag is 2 bits long. + +Empty empty # Empty field, used to represent an undefined value. + +int64 integer_value +float32 real_value +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.param.NumericValue +@union +uavcan.protocol.param.Empty empty +saturated int64 integer_value +saturated float32 real_value +******************************************************************************/ + +#define UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_NAME "uavcan.protocol.param.NumericValue" +#define UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_SIGNATURE (0xDA6D6FEA22E3587ULL) + +#define UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_MAX_SIZE ((66 + 7)/8) + +// Constants + +typedef enum +{ + UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_EMPTY, + UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE, + UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE +} uavcan_protocol_param_NumericValue_ENUM; + +typedef struct +{ + uavcan_protocol_param_NumericValue_ENUM union_tag; // union_tag indicates what field the data structure is holding + + union + { + // FieldTypes + uavcan_protocol_param_Empty empty; // + int64_t integer_value; // bit len 64 + float real_value; // float32 Saturate + + }; +} uavcan_protocol_param_NumericValue; + +extern +uint32_t uavcan_protocol_param_NumericValue_encode(uavcan_protocol_param_NumericValue* source, void* msg_buf); + +extern +int32_t uavcan_protocol_param_NumericValue_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_NumericValue* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_param_NumericValue_encode_internal(uavcan_protocol_param_NumericValue* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_param_NumericValue_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_NumericValue* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_PARAM_NUMERICVALUE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/param/Value.h b/libcanard/dsdl/uavcan/protocol/param/Value.h new file mode 100644 index 00000000..be9cfcaf --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/Value.h @@ -0,0 +1,103 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/Value.uavcan + */ + +#ifndef __UAVCAN_PROTOCOL_PARAM_VALUE +#define __UAVCAN_PROTOCOL_PARAM_VALUE + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +/******************************* Source text ********************************** +# +# Single parameter value. +# +# This is a union, which means that this structure can contain either one of the fields below. +# The structure is prefixed with tag - a selector value that indicates which particular field is encoded. +# + +@union # Tag is 3 bit long, so outer structure has 5-bit prefix to ensure proper alignment + +Empty empty # Empty field, used to represent an undefined value. + +int64 integer_value +float32 real_value # 32-bit type is used to simplify implementation on low-end systems +uint8 boolean_value # 8-bit value is used for alignment reasons +uint8[<=128] string_value # Length prefix is exactly one byte long, which ensures proper alignment of payload +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +uavcan.protocol.param.Value +@union +uavcan.protocol.param.Empty empty +saturated int64 integer_value +saturated float32 real_value +saturated uint8 boolean_value +saturated uint8[<=128] string_value +******************************************************************************/ + +#define UAVCAN_PROTOCOL_PARAM_VALUE_NAME "uavcan.protocol.param.Value" +#define UAVCAN_PROTOCOL_PARAM_VALUE_SIGNATURE (0x29F14BF484727267ULL) + +#define UAVCAN_PROTOCOL_PARAM_VALUE_MAX_SIZE ((1035 + 7)/8) + +// Constants + +#define UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE_MAX_LENGTH 128 + +typedef enum +{ + UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY, + UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE, + UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE, + UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE, + UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE +} uavcan_protocol_param_Value_ENUM; + +typedef struct +{ + uavcan_protocol_param_Value_ENUM union_tag; // union_tag indicates what field the data structure is holding + + union + { + // FieldTypes + uavcan_protocol_param_Empty empty; // + int64_t integer_value; // bit len 64 + float real_value; // float32 Saturate + uint8_t boolean_value; // bit len 8 + struct + { + uint8_t len; // Dynamic array length + uint8_t* data; // Dynamic Array 8bit[128] max items + } string_value; + + }; +} uavcan_protocol_param_Value; + +extern +uint32_t uavcan_protocol_param_Value_encode(uavcan_protocol_param_Value* source, void* msg_buf); + +extern +int32_t uavcan_protocol_param_Value_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_Value* dest, uint8_t** dyn_arr_buf); + +extern +uint32_t uavcan_protocol_param_Value_encode_internal(uavcan_protocol_param_Value* source, void* msg_buf, uint32_t offset, uint8_t root_item); + +extern +int32_t uavcan_protocol_param_Value_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_protocol_param_Value* dest, uint8_t** dyn_arr_buf, int32_t offset); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __UAVCAN_PROTOCOL_PARAM_VALUE \ No newline at end of file diff --git a/libcanard/dsdl/uavcan/protocol/param/param_Empty.c b/libcanard/dsdl/uavcan/protocol/param/param_Empty.c new file mode 100644 index 00000000..3b6ff457 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/param_Empty.c @@ -0,0 +1,53 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/Empty.uavcan + */ +#include "uavcan/protocol/param/Empty.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +uint32_t uavcan_protocol_param_Empty_encode_internal(uavcan_protocol_param_Empty* CANARD_MAYBE_UNUSED(source), + void* CANARD_MAYBE_UNUSED(msg_buf), + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + return offset; +} + +uint32_t uavcan_protocol_param_Empty_encode(uavcan_protocol_param_Empty* CANARD_MAYBE_UNUSED(source), void* CANARD_MAYBE_UNUSED(msg_buf)) +{ + return 0; +} + +int32_t uavcan_protocol_param_Empty_decode_internal(const CanardRxTransfer* CANARD_MAYBE_UNUSED(transfer), + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_Empty* CANARD_MAYBE_UNUSED(dest), + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + return offset; +} + +int32_t uavcan_protocol_param_Empty_decode(const CanardRxTransfer* CANARD_MAYBE_UNUSED(transfer), + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_Empty* CANARD_MAYBE_UNUSED(dest), + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf)) +{ + return 0; +} diff --git a/libcanard/dsdl/uavcan/protocol/param/param_ExecuteOpcode.c b/libcanard/dsdl/uavcan/protocol/param/param_ExecuteOpcode.c new file mode 100644 index 00000000..48540407 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/param_ExecuteOpcode.c @@ -0,0 +1,250 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/10.ExecuteOpcode.uavcan + */ +#include "uavcan/protocol/param/ExecuteOpcode.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_param_ExecuteOpcodeRequest_encode_internal(uavcan_protocol_param_ExecuteOpcodeRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->opcode); // 255 + offset += 8; + + source->argument = CANARD_INTERNAL_SATURATE(source->argument, 140737488355327) + canardEncodeScalar(msg_buf, offset, 48, (void*)&source->argument); // 140737488355327 + offset += 48; + + return offset; +} + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_param_ExecuteOpcodeRequest_encode(uavcan_protocol_param_ExecuteOpcodeRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_param_ExecuteOpcodeRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_ExecuteOpcodeRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_ExecuteOpcodeRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_ExecuteOpcodeRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->opcode); + if (ret != 8) + { + goto uavcan_protocol_param_ExecuteOpcodeRequest_error_exit; + } + offset += 8; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 48, true, (void*)&dest->argument); + if (ret != 48) + { + goto uavcan_protocol_param_ExecuteOpcodeRequest_error_exit; + } + offset += 48; + return offset; + +uavcan_protocol_param_ExecuteOpcodeRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_ExecuteOpcodeRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_ExecuteOpcodeRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_param_ExecuteOpcodeRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_param_ExecuteOpcodeRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_param_ExecuteOpcodeRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_param_ExecuteOpcodeResponse_encode_internal(uavcan_protocol_param_ExecuteOpcodeResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + source->argument = CANARD_INTERNAL_SATURATE(source->argument, 140737488355327) + canardEncodeScalar(msg_buf, offset, 48, (void*)&source->argument); // 140737488355327 + offset += 48; + + source->ok = CANARD_INTERNAL_SATURATE_UNSIGNED(source->ok, 1) + canardEncodeScalar(msg_buf, offset, 1, (void*)&source->ok); // 1 + offset += 1; + + return offset; +} + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_param_ExecuteOpcodeResponse_encode(uavcan_protocol_param_ExecuteOpcodeResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_param_ExecuteOpcodeResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_ExecuteOpcodeResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_ExecuteOpcodeResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_ExecuteOpcodeResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 48, true, (void*)&dest->argument); + if (ret != 48) + { + goto uavcan_protocol_param_ExecuteOpcodeResponse_error_exit; + } + offset += 48; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 1, false, (void*)&dest->ok); + if (ret != 1) + { + goto uavcan_protocol_param_ExecuteOpcodeResponse_error_exit; + } + offset += 1; + return offset; + +uavcan_protocol_param_ExecuteOpcodeResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_param_ExecuteOpcodeResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_ExecuteOpcodeResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_ExecuteOpcodeResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_param_ExecuteOpcodeResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_param_ExecuteOpcodeResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_param_ExecuteOpcodeResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/param/param_GetSet.c b/libcanard/dsdl/uavcan/protocol/param/param_GetSet.c new file mode 100644 index 00000000..b65bb91b --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/param_GetSet.c @@ -0,0 +1,431 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/11.GetSet.uavcan + */ +#include "uavcan/protocol/param/GetSet.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_param_GetSetRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_param_GetSetRequest_encode_internal(uavcan_protocol_param_GetSetRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + source->index = CANARD_INTERNAL_SATURATE_UNSIGNED(source->index, 8191) + canardEncodeScalar(msg_buf, offset, 13, (void*)&source->index); // 8191 + offset += 13; + + // Compound + offset = uavcan_protocol_param_Value_encode_internal(&source->value, msg_buf, offset, 0); + + // Dynamic Array (name) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 7, (void*)&source->name.len); + offset += 7; + } + + // - Add array items + for (c = 0; c < source->name.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->name.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_param_GetSetRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_param_GetSetRequest_encode(uavcan_protocol_param_GetSetRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_param_GetSetRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_param_GetSetRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_GetSetRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_GetSetRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_GetSetRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 13, false, (void*)&dest->index); + if (ret != 13) + { + goto uavcan_protocol_param_GetSetRequest_error_exit; + } + offset += 13; + + // Compound + offset = uavcan_protocol_param_Value_decode_internal(transfer, payload_len, &dest->value, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_param_GetSetRequest_error_exit; + } + + // Dynamic Array (name) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->name.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 7 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 7, + false, + (void*)&dest->name.len); // 255 + if (ret != 7) + { + goto uavcan_protocol_param_GetSetRequest_error_exit; + } + offset += 7; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->name.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->name.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_param_GetSetRequest_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_param_GetSetRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_param_GetSetRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_GetSetRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_GetSetRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_param_GetSetRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_param_GetSetRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_param_GetSetRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_param_GetSetResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_param_GetSetResponse_encode_internal(uavcan_protocol_param_GetSetResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + // Void5 + offset += 5; + + // Compound + offset = uavcan_protocol_param_Value_encode_internal(&source->value, msg_buf, offset, 0); + + // Void5 + offset += 5; + + // Compound + offset = uavcan_protocol_param_Value_encode_internal(&source->default_value, msg_buf, offset, 0); + + // Void6 + offset += 6; + + // Compound + offset = uavcan_protocol_param_NumericValue_encode_internal(&source->max_value, msg_buf, offset, 0); + + // Void6 + offset += 6; + + // Compound + offset = uavcan_protocol_param_NumericValue_encode_internal(&source->min_value, msg_buf, offset, 0); + + // Dynamic Array (name) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 7, (void*)&source->name.len); + offset += 7; + } + + // - Add array items + for (c = 0; c < source->name.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->name.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_param_GetSetResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_param_GetSetResponse_encode(uavcan_protocol_param_GetSetResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_param_GetSetResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_param_GetSetResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_GetSetResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_GetSetResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_GetSetResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + // Void5 + offset += 5; + + // Compound + offset = uavcan_protocol_param_Value_decode_internal(transfer, payload_len, &dest->value, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_param_GetSetResponse_error_exit; + } + + // Void5 + offset += 5; + + // Compound + offset = uavcan_protocol_param_Value_decode_internal(transfer, payload_len, &dest->default_value, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_param_GetSetResponse_error_exit; + } + + // Void6 + offset += 6; + + // Compound + offset = uavcan_protocol_param_NumericValue_decode_internal(transfer, payload_len, &dest->max_value, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_param_GetSetResponse_error_exit; + } + + // Void6 + offset += 6; + + // Compound + offset = uavcan_protocol_param_NumericValue_decode_internal(transfer, payload_len, &dest->min_value, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_param_GetSetResponse_error_exit; + } + + // Dynamic Array (name) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->name.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 7 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 7, + false, + (void*)&dest->name.len); // 255 + if (ret != 7) + { + goto uavcan_protocol_param_GetSetResponse_error_exit; + } + offset += 7; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->name.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->name.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_param_GetSetResponse_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_param_GetSetResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_param_GetSetResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_GetSetResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_GetSetResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_param_GetSetResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_param_GetSetResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_param_GetSetResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/param/param_NumericValue.c b/libcanard/dsdl/uavcan/protocol/param/param_NumericValue.c new file mode 100644 index 00000000..6225a8f9 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/param_NumericValue.c @@ -0,0 +1,174 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/NumericValue.uavcan + */ +#include "uavcan/protocol/param/NumericValue.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_param_NumericValue_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_param_NumericValue_encode_internal(uavcan_protocol_param_NumericValue* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + // Max Union Tag Value + CANARD_ASSERT(source->union_tag <= 2); + + // Union Tag 2 bits + canardEncodeScalar(msg_buf, offset, 2, (void*)&source->union_tag); // 2 bits + offset += 2; + + if (source->union_tag == 0) { + // Compound + offset = uavcan_protocol_param_Empty_encode_internal(&source->empty, msg_buf, offset, 0); + } + else if (source->union_tag == 1) { + canardEncodeScalar(msg_buf, offset, 64, (void*)&source->integer_value); // 9223372036854775807 + offset += 64; + + } + else if (source->union_tag == 2) { + canardEncodeScalar(msg_buf, offset, 32, (void*)&source->real_value); // 2147483647 + offset += 32; + + } + + return offset; +} + +/** + * @brief uavcan_protocol_param_NumericValue_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_param_NumericValue_encode(uavcan_protocol_param_NumericValue* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_param_NumericValue_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_param_NumericValue_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_NumericValue dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_NumericValue_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_NumericValue* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + // Get Union Tag + ret = canardDecodeScalar(transfer, (uint32_t)offset, 2, false, (void*)&dest->union_tag); // 2 + if (ret != 2) + { + goto uavcan_protocol_param_NumericValue_error_exit; + } + offset += 2; + + if (dest->union_tag == 0) + { + // Compound + offset = uavcan_protocol_param_Empty_decode_internal(transfer, payload_len, &dest->empty, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_param_NumericValue_error_exit; + } + } + else if (dest->union_tag == 1) + { + ret = canardDecodeScalar(transfer, (uint32_t)offset, 64, true, (void*)&dest->integer_value); + if (ret != 64) + { + goto uavcan_protocol_param_NumericValue_error_exit; + } + offset += 64; + } + else if (dest->union_tag == 2) + { + ret = canardDecodeScalar(transfer, (uint32_t)offset, 32, false, (void*)&dest->real_value); + if (ret != 32) + { + goto uavcan_protocol_param_NumericValue_error_exit; + } + offset += 32; + } + return offset; + +uavcan_protocol_param_NumericValue_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_param_NumericValue_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_NumericValue dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_NumericValue_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_param_NumericValue* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_param_NumericValue); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_param_NumericValue_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/param/param_Value.c b/libcanard/dsdl/uavcan/protocol/param/param_Value.c new file mode 100644 index 00000000..2068cb40 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/param/param_Value.c @@ -0,0 +1,258 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/param/Value.uavcan + */ +#include "uavcan/protocol/param/Value.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_param_Value_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_param_Value_encode_internal(uavcan_protocol_param_Value* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + // Max Union Tag Value + CANARD_ASSERT(source->union_tag <= 4); + uint32_t c = 0; + + // Union Tag 3 bits + canardEncodeScalar(msg_buf, offset, 3, (void*)&source->union_tag); // 3 bits + offset += 3; + + if (source->union_tag == 0) { + // Compound + offset = uavcan_protocol_param_Empty_encode_internal(&source->empty, msg_buf, offset, 0); + } + else if (source->union_tag == 1) { + canardEncodeScalar(msg_buf, offset, 64, (void*)&source->integer_value); // 9223372036854775807 + offset += 64; + + } + else if (source->union_tag == 2) { + canardEncodeScalar(msg_buf, offset, 32, (void*)&source->real_value); // 2147483647 + offset += 32; + + } + else if (source->union_tag == 3) { + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->boolean_value); // 255 + offset += 8; + + } + else if (source->union_tag == 4) { + // Dynamic Array (string_value) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->string_value.len); + offset += 8; + } + + // - Add array items + for (c = 0; c < source->string_value.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->string_value.data + c));// 255 + offset += 8; + } + + } + + return offset; +} + +/** + * @brief uavcan_protocol_param_Value_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_param_Value_encode(uavcan_protocol_param_Value* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_param_Value_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_param_Value_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_Value dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_Value_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_param_Value* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + // Get Union Tag + ret = canardDecodeScalar(transfer, (uint32_t)offset, 3, false, (void*)&dest->union_tag); // 3 + if (ret != 3) + { + goto uavcan_protocol_param_Value_error_exit; + } + offset += 3; + + if (dest->union_tag == 0) + { + // Compound + offset = uavcan_protocol_param_Empty_decode_internal(transfer, payload_len, &dest->empty, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_param_Value_error_exit; + } + } + else if (dest->union_tag == 1) + { + ret = canardDecodeScalar(transfer, (uint32_t)offset, 64, true, (void*)&dest->integer_value); + if (ret != 64) + { + goto uavcan_protocol_param_Value_error_exit; + } + offset += 64; + } + else if (dest->union_tag == 2) + { + ret = canardDecodeScalar(transfer, (uint32_t)offset, 32, false, (void*)&dest->real_value); + if (ret != 32) + { + goto uavcan_protocol_param_Value_error_exit; + } + offset += 32; + } + else if (dest->union_tag == 3) + { + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->boolean_value); + if (ret != 8) + { + goto uavcan_protocol_param_Value_error_exit; + } + offset += 8; + } + else if (dest->union_tag == 4) + { + // Dynamic Array (string_value) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->string_value.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 8 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)&dest->string_value.len); // 255 + if (ret != 8) + { + goto uavcan_protocol_param_Value_error_exit; + } + offset += 8; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->string_value.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->string_value.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_param_Value_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + } + return offset; + +uavcan_protocol_param_Value_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_param_Value_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_param_Value dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_param_Value_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_param_Value* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_param_Value); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_param_Value_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/protocol_GetNodeInfo.c b/libcanard/dsdl/uavcan/protocol/protocol_GetNodeInfo.c new file mode 100755 index 00000000..8a078562 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/protocol_GetNodeInfo.c @@ -0,0 +1,245 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/AION/ardupilot_private/modules/uavcan/dsdl/uavcan/protocol/1.GetNodeInfo.uavcan + */ +#include "uavcan/protocol/GetNodeInfo.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +uint32_t uavcan_protocol_GetNodeInfoRequest_encode_internal(uavcan_protocol_GetNodeInfoRequest* CANARD_MAYBE_UNUSED(source), + void* CANARD_MAYBE_UNUSED(msg_buf), + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + return offset; +} + +uint32_t uavcan_protocol_GetNodeInfoRequest_encode(uavcan_protocol_GetNodeInfoRequest* CANARD_MAYBE_UNUSED(source), void* CANARD_MAYBE_UNUSED(msg_buf)) +{ + return 0; +} + +int32_t uavcan_protocol_GetNodeInfoRequest_decode_internal(const CanardRxTransfer* CANARD_MAYBE_UNUSED(transfer), + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_GetNodeInfoRequest* CANARD_MAYBE_UNUSED(dest), + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + return offset; +} + +int32_t uavcan_protocol_GetNodeInfoRequest_decode(const CanardRxTransfer* CANARD_MAYBE_UNUSED(transfer), + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_GetNodeInfoRequest* CANARD_MAYBE_UNUSED(dest), + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf)) +{ + return 0; +} + +/** + * @brief uavcan_protocol_GetNodeInfoResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_GetNodeInfoResponse_encode_internal(uavcan_protocol_GetNodeInfoResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + // Compound + offset = uavcan_protocol_NodeStatus_encode_internal(&source->status, msg_buf, offset, 0); + + // Compound + offset = uavcan_protocol_SoftwareVersion_encode_internal(&source->software_version, msg_buf, offset, 0); + + // Compound + offset = uavcan_protocol_HardwareVersion_encode_internal(&source->hardware_version, msg_buf, offset, 0); + + // Dynamic Array (name) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 7, (void*)&source->name.len); + offset += 7; + } + + // - Add array items + for (c = 0; c < source->name.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->name.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_GetNodeInfoResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_GetNodeInfoResponse_encode(uavcan_protocol_GetNodeInfoResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_GetNodeInfoResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_GetNodeInfoResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_GetNodeInfoResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_GetNodeInfoResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_GetNodeInfoResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + // Compound + offset = uavcan_protocol_NodeStatus_decode_internal(transfer, payload_len, &dest->status, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_GetNodeInfoResponse_error_exit; + } + + // Compound + offset = uavcan_protocol_SoftwareVersion_decode_internal(transfer, payload_len, &dest->software_version, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_GetNodeInfoResponse_error_exit; + } + + // Compound + offset = uavcan_protocol_HardwareVersion_decode_internal(transfer, payload_len, &dest->hardware_version, dyn_arr_buf, offset); + if (offset < 0) + { + ret = offset; + goto uavcan_protocol_GetNodeInfoResponse_error_exit; + } + + // Dynamic Array (name) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->name.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 7 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 7, + false, + (void*)&dest->name.len); // 255 + if (ret != 7) + { + goto uavcan_protocol_GetNodeInfoResponse_error_exit; + } + offset += 7; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->name.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->name.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_GetNodeInfoResponse_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_GetNodeInfoResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_GetNodeInfoResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_GetNodeInfoResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_GetNodeInfoResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_GetNodeInfoResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_GetNodeInfoResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_GetNodeInfoResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/protocol_HardwareVersion.c b/libcanard/dsdl/uavcan/protocol/protocol_HardwareVersion.c new file mode 100644 index 00000000..a83f62fe --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/protocol_HardwareVersion.c @@ -0,0 +1,220 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/HardwareVersion.uavcan + */ +#include "uavcan/protocol/HardwareVersion.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_HardwareVersion_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_HardwareVersion_encode_internal(uavcan_protocol_HardwareVersion* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + uint32_t c = 0; + + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->major); // 255 + offset += 8; + + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->minor); // 255 + offset += 8; + + // Static array (unique_id) + for (c = 0; c < 16; c++) + { + canardEncodeScalar(msg_buf, offset, 8, (void*)(source->unique_id + c)); // 255 + offset += 8; + } + + // Dynamic Array (certificate_of_authenticity) + if (! root_item) + { + // - Add array length + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->certificate_of_authenticity.len); + offset += 8; + } + + // - Add array items + for (c = 0; c < source->certificate_of_authenticity.len; c++) + { + canardEncodeScalar(msg_buf, + offset, + 8, + (void*)(source->certificate_of_authenticity.data + c));// 255 + offset += 8; + } + + return offset; +} + +/** + * @brief uavcan_protocol_HardwareVersion_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_HardwareVersion_encode(uavcan_protocol_HardwareVersion* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_HardwareVersion_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_HardwareVersion_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_HardwareVersion dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_HardwareVersion_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_HardwareVersion* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + uint32_t c = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->major); + if (ret != 8) + { + goto uavcan_protocol_HardwareVersion_error_exit; + } + offset += 8; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->minor); + if (ret != 8) + { + goto uavcan_protocol_HardwareVersion_error_exit; + } + offset += 8; + + // Static array (unique_id) + for (c = 0; c < 16; c++) + { + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)(dest->unique_id + c)); + if (ret != 8) + { + goto uavcan_protocol_HardwareVersion_error_exit; + } + offset += 8; + } + + // Dynamic Array (certificate_of_authenticity) + // - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization + if (payload_len) + { + // - Calculate Array length from MSG length + dest->certificate_of_authenticity.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size + } + else + { + // - Array length 8 bits + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)&dest->certificate_of_authenticity.len); // 255 + if (ret != 8) + { + goto uavcan_protocol_HardwareVersion_error_exit; + } + offset += 8; + } + + // - Get Array + if (dyn_arr_buf) + { + dest->certificate_of_authenticity.data = (uint8_t*)*dyn_arr_buf; + } + + for (c = 0; c < dest->certificate_of_authenticity.len; c++) + { + if (dyn_arr_buf) + { + ret = canardDecodeScalar(transfer, + (uint32_t)offset, + 8, + false, + (void*)*dyn_arr_buf); // 255 + if (ret != 8) + { + goto uavcan_protocol_HardwareVersion_error_exit; + } + *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1); + } + offset += 8; + } + return offset; + +uavcan_protocol_HardwareVersion_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_HardwareVersion_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_HardwareVersion dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_HardwareVersion_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_HardwareVersion* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_HardwareVersion); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_HardwareVersion_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/protocol_NodeStatus.c b/libcanard/dsdl/uavcan/protocol/protocol_NodeStatus.c new file mode 100755 index 00000000..46fbc9c1 --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/protocol_NodeStatus.c @@ -0,0 +1,168 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/AION/ardupilot_private/modules/uavcan/dsdl/uavcan/protocol/341.NodeStatus.uavcan + */ +#include "uavcan/protocol/NodeStatus.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_NodeStatus_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_NodeStatus_encode_internal(uavcan_protocol_NodeStatus* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + canardEncodeScalar(msg_buf, offset, 32, (void*)&source->uptime_sec); // 4294967295 + offset += 32; + + source->health = CANARD_INTERNAL_SATURATE_UNSIGNED(source->health, 3) + canardEncodeScalar(msg_buf, offset, 2, (void*)&source->health); // 3 + offset += 2; + + source->mode = CANARD_INTERNAL_SATURATE_UNSIGNED(source->mode, 7) + canardEncodeScalar(msg_buf, offset, 3, (void*)&source->mode); // 7 + offset += 3; + + source->sub_mode = CANARD_INTERNAL_SATURATE_UNSIGNED(source->sub_mode, 7) + canardEncodeScalar(msg_buf, offset, 3, (void*)&source->sub_mode); // 7 + offset += 3; + + canardEncodeScalar(msg_buf, offset, 16, (void*)&source->vendor_specific_status_code); // 65535 + offset += 16; + + return offset; +} + +/** + * @brief uavcan_protocol_NodeStatus_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_NodeStatus_encode(uavcan_protocol_NodeStatus* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_NodeStatus_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_NodeStatus_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_NodeStatus dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_NodeStatus_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_NodeStatus* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 32, false, (void*)&dest->uptime_sec); + if (ret != 32) + { + goto uavcan_protocol_NodeStatus_error_exit; + } + offset += 32; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 2, false, (void*)&dest->health); + if (ret != 2) + { + goto uavcan_protocol_NodeStatus_error_exit; + } + offset += 2; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 3, false, (void*)&dest->mode); + if (ret != 3) + { + goto uavcan_protocol_NodeStatus_error_exit; + } + offset += 3; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 3, false, (void*)&dest->sub_mode); + if (ret != 3) + { + goto uavcan_protocol_NodeStatus_error_exit; + } + offset += 3; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&dest->vendor_specific_status_code); + if (ret != 16) + { + goto uavcan_protocol_NodeStatus_error_exit; + } + offset += 16; + return offset; + +uavcan_protocol_NodeStatus_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_NodeStatus_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_NodeStatus dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_NodeStatus_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_NodeStatus* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_NodeStatus); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_NodeStatus_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/protocol_RestartNode.c b/libcanard/dsdl/uavcan/protocol/protocol_RestartNode.c new file mode 100644 index 00000000..c37f0fcf --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/protocol_RestartNode.c @@ -0,0 +1,229 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/5.RestartNode.uavcan + */ +#include "uavcan/protocol/RestartNode.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_RestartNodeRequest_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_RestartNodeRequest_encode_internal(uavcan_protocol_RestartNodeRequest* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + source->magic_number = CANARD_INTERNAL_SATURATE_UNSIGNED(source->magic_number, 1099511627775) + canardEncodeScalar(msg_buf, offset, 40, (void*)&source->magic_number); // 1099511627775 + offset += 40; + + return offset; +} + +/** + * @brief uavcan_protocol_RestartNodeRequest_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_RestartNodeRequest_encode(uavcan_protocol_RestartNodeRequest* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_RestartNodeRequest_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_RestartNodeRequest_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_RestartNodeRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_RestartNodeRequest_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_RestartNodeRequest* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 40, false, (void*)&dest->magic_number); + if (ret != 40) + { + goto uavcan_protocol_RestartNodeRequest_error_exit; + } + offset += 40; + return offset; + +uavcan_protocol_RestartNodeRequest_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_RestartNodeRequest_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_RestartNodeRequest dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_RestartNodeRequest_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_RestartNodeRequest* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_RestartNodeRequest); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_RestartNodeRequest_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} + +/** + * @brief uavcan_protocol_RestartNodeResponse_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_RestartNodeResponse_encode_internal(uavcan_protocol_RestartNodeResponse* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + source->ok = CANARD_INTERNAL_SATURATE_UNSIGNED(source->ok, 1) + canardEncodeScalar(msg_buf, offset, 1, (void*)&source->ok); // 1 + offset += 1; + + return offset; +} + +/** + * @brief uavcan_protocol_RestartNodeResponse_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_RestartNodeResponse_encode(uavcan_protocol_RestartNodeResponse* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_RestartNodeResponse_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_RestartNodeResponse_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_RestartNodeResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_RestartNodeResponse_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_RestartNodeResponse* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 1, false, (void*)&dest->ok); + if (ret != 1) + { + goto uavcan_protocol_RestartNodeResponse_error_exit; + } + offset += 1; + return offset; + +uavcan_protocol_RestartNodeResponse_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_RestartNodeResponse_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_RestartNodeResponse dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_RestartNodeResponse_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_RestartNodeResponse* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_RestartNodeResponse); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_RestartNodeResponse_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +} diff --git a/libcanard/dsdl/uavcan/protocol/protocol_SoftwareVersion.c b/libcanard/dsdl/uavcan/protocol/protocol_SoftwareVersion.c new file mode 100644 index 00000000..df8db60c --- /dev/null +++ b/libcanard/dsdl/uavcan/protocol/protocol_SoftwareVersion.c @@ -0,0 +1,165 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /Users/jaime/Developer/ardupilot/modules/uavcan/dsdl/uavcan/protocol/SoftwareVersion.uavcan + */ +#include "uavcan/protocol/SoftwareVersion.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief uavcan_protocol_SoftwareVersion_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t uavcan_protocol_SoftwareVersion_encode_internal(uavcan_protocol_SoftwareVersion* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item)) +{ + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->major); // 255 + offset += 8; + + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->minor); // 255 + offset += 8; + + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->optional_field_flags); // 255 + offset += 8; + + canardEncodeScalar(msg_buf, offset, 32, (void*)&source->vcs_commit); // 4294967295 + offset += 32; + + canardEncodeScalar(msg_buf, offset, 64, (void*)&source->image_crc); // 18446744073709551615 + offset += 64; + + return offset; +} + +/** + * @brief uavcan_protocol_SoftwareVersion_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t uavcan_protocol_SoftwareVersion_encode(uavcan_protocol_SoftwareVersion* source, void* msg_buf) +{ + uint32_t offset = 0; + + offset = uavcan_protocol_SoftwareVersion_encode_internal(source, msg_buf, offset, 1); + + return (offset + 7 ) / 8; +} + +/** + * @brief uavcan_protocol_SoftwareVersion_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_SoftwareVersion dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_SoftwareVersion_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + uavcan_protocol_SoftwareVersion* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset) +{ + int32_t ret = 0; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->major); + if (ret != 8) + { + goto uavcan_protocol_SoftwareVersion_error_exit; + } + offset += 8; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->minor); + if (ret != 8) + { + goto uavcan_protocol_SoftwareVersion_error_exit; + } + offset += 8; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->optional_field_flags); + if (ret != 8) + { + goto uavcan_protocol_SoftwareVersion_error_exit; + } + offset += 8; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 32, false, (void*)&dest->vcs_commit); + if (ret != 32) + { + goto uavcan_protocol_SoftwareVersion_error_exit; + } + offset += 32; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 64, false, (void*)&dest->image_crc); + if (ret != 64) + { + goto uavcan_protocol_SoftwareVersion_error_exit; + } + offset += 64; + return offset; + +uavcan_protocol_SoftwareVersion_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief uavcan_protocol_SoftwareVersion_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * uavcan_protocol_SoftwareVersion dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t uavcan_protocol_SoftwareVersion_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + uavcan_protocol_SoftwareVersion* dest, + uint8_t** dyn_arr_buf) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(uavcan_protocol_SoftwareVersion); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = uavcan_protocol_SoftwareVersion_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset); + + return ret; +}