zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Remove ucannode_gps_demo since it's superseded by uavcan_v1

master
Peter van der Perk 3 years ago committed by Daniel Agar
parent
ce909b23b1
commit
ce10dd90e7
18 changed files with 0 additions and 2275 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      .gitmodules
  2. 83
      src/drivers/uavcannode_gps_demo/CMakeLists.txt
  3. 5
      src/drivers/uavcannode_gps_demo/Kconfig
  4. 469
      src/drivers/uavcannode_gps_demo/canard_main.c
  5. 1
      src/drivers/uavcannode_gps_demo/libcanard
  6. 12
      src/drivers/uavcannode_gps_demo/libcancl/README.md
  7. 264
      src/drivers/uavcannode_gps_demo/libcancl/pnp.c
  8. 74
      src/drivers/uavcannode_gps_demo/libcancl/pnp.h
  9. 282
      src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c
  10. 49
      src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h
  11. 6
      src/drivers/uavcannode_gps_demo/libcancl/time.h
  12. 498
      src/drivers/uavcannode_gps_demo/o1heap.c
  13. 142
      src/drivers/uavcannode_gps_demo/o1heap.h
  14. 1
      src/drivers/uavcannode_gps_demo/public_regulated_data_types
  15. 172
      src/drivers/uavcannode_gps_demo/socketcan.c
  16. 69
      src/drivers/uavcannode_gps_demo/socketcan.h
  17. 108
      src/drivers/uavcannode_gps_demo/uorb_converter.c
  18. 34
      src/drivers/uavcannode_gps_demo/uorb_converter.h

6
.gitmodules vendored
Unescape View File

@ -42,12 +42,6 @@ @@ -42,12 +42,6 @@
[submodule "src/drivers/uavcan_v1/public_regulated_data_types"]
path = src/drivers/uavcan_v1/public_regulated_data_types
url = https://github.com/UAVCAN/public_regulated_data_types.git
[submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"]
path = src/drivers/uavcannode_gps_demo/public_regulated_data_types
url = https://github.com/UAVCAN/public_regulated_data_types.git
[submodule "src/drivers/uavcannode_gps_demo/libcanard"]
path = src/drivers/uavcannode_gps_demo/libcanard
url = https://github.com/UAVCAN/libcanard.git
[submodule "src/drivers/uavcan_v1/legacy_data_types"]
path = src/drivers/uavcan_v1/legacy_data_types
url = https://github.com/PX4/public_regulated_data_types.git

83
src/drivers/uavcannode_gps_demo/CMakeLists.txt
Unescape View File

@ -1,83 +0,0 @@ @@ -1,83 +0,0 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
set(LIBCANARD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/libcanard)
set(DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/public_regulated_data_types)
px4_add_git_submodule(TARGET git_libcanard PATH ${LIBCANARD_DIR})
px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR})
find_program(NNVG_PATH nnvg)
if(NNVG_PATH)
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg)
execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan)
else()
message(FATAL_ERROR "UAVCAN Nunavut nnvg not found")
endif()
add_definitions(
-DCONFIG_EXAMPLES_LIBCANARDV1_DEV="can0"
-DCONFIG_EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE=8192
-DCONFIG_EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE=5000
-DCONFIG_EXAMPLES_LIBCANARDV1_DAEMON_PRIORITY=100
-DCANARD_DSDL_CONFIG_LITTLE_ENDIAN=1
)
px4_add_module(
MODULE drivers__uavcannode-gps-demo
MAIN uavcannode_gps_demo
COMPILE_FLAGS
-Wno-error
-DUINT32_C\(x\)=__UINT32_C\(x\)
INCLUDES
${LIBCANARD_DIR}/libcanard/
${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
SRCS
canard_main.c
socketcan.c
socketcan.h
uorb_converter.c
uorb_converter.h
o1heap.c
o1heap.h
libcancl/pnp.c
libcancl/registerinterface.c
${LIBCANARD_DIR}/libcanard/canard.c
${LIBCANARD_DIR}/libcanard/canard.h
DEPENDS
git_libcanard
git_public_regulated_data_types
drivers_bootloaders
version
)

5
src/drivers/uavcannode_gps_demo/Kconfig
Unescape View File

@ -1,5 +0,0 @@ @@ -1,5 +0,0 @@
menuconfig DRIVERS_UAVCANNODE_GPS_DEMO
bool "uavcannode_gps_demo"
default n
---help---
Enable support for uavcannode_gps_demo

469
src/drivers/uavcannode_gps_demo/canard_main.c
Unescape View File

@ -1,469 +0,0 @@ @@ -1,469 +0,0 @@
/****************************************************************************
* examples/canard/canard_main.c
*
* Copyright (C) 2016 ETH Zuerich. All rights reserved.
* Author: Matthias Renner <rennerm@ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <canard.h>
#include <canard_dsdl.h>
#include <sched.h>
#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <string.h>
#include <assert.h>
#include <errno.h>
#include <net/if.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <poll.h>
#include <nuttx/can.h>
#include <netpacket/can.h>
#include "socketcan.h"
#include "o1heap.h"
#include "uorb_converter.h"
#include "libcancl/pnp.h"
#include "libcancl/registerinterface.h"
#include "boot_app_shared.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Arena for memory allocation, used by the library */
#define O1_HEAP_SIZE CONFIG_EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE
#define UNIQUE_ID_LENGTH_BYTES 16
/****************************************************************************
* Private Data
****************************************************************************/
/*
* This is the AppImageDescriptor used
* by the make_can_boot_descriptor.py tool to set
* the application image's descriptor so that the
* uavcan bootloader has the ability to validate the
* image crc, size etc of this application
*/
boot_app_shared_section app_descriptor_t AppDescriptor = {
.signature = APP_DESCRIPTOR_SIGNATURE,
.image_crc = 0,
.image_size = 0,
.git_hash = 0,
.major_version = APP_VERSION_MAJOR,
.minor_version = APP_VERSION_MINOR,
.board_id = HW_VERSION_MAJOR << 8 | HW_VERSION_MINOR,
.reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
};
O1HeapInstance *my_allocator;
static uint8_t uavcan_heap[O1_HEAP_SIZE]
__attribute__((aligned(O1HEAP_ALIGNMENT)));
static uint8_t unique_id[UNIQUE_ID_LENGTH_BYTES] = { //FIXME real HW ID
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01
};
/* Node status variables */
static bool g_canard_daemon_started;
static int16_t gps_uorb_port_id = -1;
static int16_t gps_fix_port_id = -1;
static int16_t gps_aux_port_id = -1;
struct pollfd fd;
/****************************************************************************
* private functions
****************************************************************************/
//TODO move this to a seperate file probably
int32_t set_gps_uorb_port_id(uavcan_register_Value_1_0 *value)
{
if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16
//TODO check validity
printf("Master: set uORB portID to %i\n", value->natural16.value.elements[0]);
gps_uorb_port_id = value->natural16.value.elements[0];
return 0;
}
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
uavcan_register_Value_1_0 get_gps_uorb_port_id()
{
void *dataReturn;
uavcan_register_Value_1_0 value;
value.natural16.value.elements[0] = gps_uorb_port_id;
value.natural16.value.count = 1;
value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this??
return value;
}
int32_t set_gps_fix_port_id(uavcan_register_Value_1_0 *value)
{
if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16
//TODO check validity
printf("Master: set FIX portID to %i\n", value->natural16.value.elements[0]);
gps_fix_port_id = value->natural16.value.elements[0];
return 0;
}
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
uavcan_register_Value_1_0 get_gps_fix_port_id()
{
void *dataReturn;
uavcan_register_Value_1_0 value;
value.natural16.value.elements[0] = gps_fix_port_id;
value.natural16.value.count = 1;
value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this??
return value;
}
int32_t set_gps_aux_port_id(uavcan_register_Value_1_0 *value)
{
if (uavcan_register_Value_1_0_is_natural16_(value) && value->natural16.value.count == 1) { // Natural 16
//TODO check validity
printf("Master: set AUX portID to %i\n", value->natural16.value.elements[0]);
gps_fix_port_id = value->natural16.value.elements[0];
return 0;
}
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
uavcan_register_Value_1_0 get_gps_aux_port_id()
{
void *dataReturn;
uavcan_register_Value_1_0 value;
value.natural16.value.elements[0] = gps_aux_port_id;
value.natural16.value.count = 1;
value._tag_ = 10; // TODO does nunavut generate ENUM/defines for this??
return value;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: memAllocate
*
* Description:
*
****************************************************************************/
static void *memAllocate(CanardInstance *const ins, const size_t amount)
{
(void) ins;
return o1heapAllocate(my_allocator, amount);
}
/****************************************************************************
* Name: memFree
*
* Description:
*
****************************************************************************/
static void memFree(CanardInstance *const ins, void *const pointer)
{
(void) ins;
o1heapFree(my_allocator, pointer);
}
/****************************************************************************
* Name: getMonotonicTimestampUSec
*
* Description:
*
****************************************************************************/
uint64_t getMonotonicTimestampUSec(void)
{
int ret;
struct timespec ts;
memset(&ts, 0, sizeof(ts));
ret = clock_gettime(CLOCK_MONOTONIC, &ts);
if (ret != 0) {
PX4_ERR("clock error %i", ret);
abort();
}
return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
}
/****************************************************************************
* Name: processTxRxOnce
*
* Description:
* Transmits all frames from the TX queue, receives up to one frame.
*
****************************************************************************/
void processTxRxOnce(CanardInstance *ins, CanardSocketInstance *sock_ins, int timeout_msec)
{
int32_t result;
/* Transmitting */
for (const CanardFrame *txf = NULL; (txf = canardTxPeek(ins)) != NULL;) { // Look at the top of the TX queue.
if (txf->timestamp_usec > getMonotonicTimestampUSec()) { // Check if the frame has timed out.
if (socketcanTransmit(sock_ins, txf) == 0) { // Send the frame. Redundant interfaces may be used here.
break; // If the driver is busy, break and retry later.
}
} else {
printf("Timeout??\n");
}
canardTxPop(ins); // Remove the frame from the queue after it's transmitted.
ins->memory_free(ins, (CanardFrame *)txf); // Deallocate the dynamic memory afterwards.
}
/* Poll receive */
if (poll(&fd, 1, timeout_msec) <= 0) {
return;
}
/* Receiving */
CanardFrame received_frame;
socketcanReceive(sock_ins, &received_frame);
CanardTransfer receive;
result = canardRxAccept(ins,
&received_frame, // The CAN frame received from the bus.
0, // If the transport is not redundant, use 0.
&receive);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if
// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
// Reception of an invalid frame is NOT an error.
fprintf(stderr, "Receive error %" PRId32 "\n", result);
} else if (result == 1) {
printf("Receive portId %i\n", receive.port_id);
if (receive.port_id == PNPGetPortID(ins)) {
PNPProcess(ins, &receive);
} else {
uavcan_register_interface_process(ins, &receive);
}
ins->memory_free(ins, (void *)receive.payload); // Deallocate the dynamic memory afterwards.
} else {
// printf("RX canard %d\r\n", result);
// Nothing to do.
// The received frame is either invalid or it's a non-last frame of a multi-frame transfer.
// Reception of an invalid frame is NOT reported as an error because it is not an error.
}
}
/****************************************************************************
* Name: canard_daemon
*
* Description:
*
****************************************************************************/
static int canard_daemon(int argc, char *argv[])
{
int errval = 0;
int can_fd = 0;
if (argc > 2) {
for (int args = 2; args < argc; args++) {
if (!strcmp(argv[args], "canfd")) {
can_fd = 1;
}
}
}
my_allocator = o1heapInit(&uavcan_heap, O1_HEAP_SIZE, NULL, NULL);
if (my_allocator == NULL) {
printf("o1heapInit failed with size %d\n", O1_HEAP_SIZE);
errval = 2;
goto errout_with_dev;
}
CanardInstance ins = canardInit(&memAllocate, &memFree);
if (can_fd) {
ins.mtu_bytes = CANARD_MTU_CAN_FD;
} else {
ins.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
}
/* Open the CAN device for reading */
CanardSocketInstance sock_ins;
socketcanOpen(&sock_ins, CONFIG_EXAMPLES_LIBCANARDV1_DEV, can_fd);
/* setup poll fd */
fd.fd = sock_ins.s;
fd.events = POLLIN;
if (sock_ins.s < 0) {
printf("canard_daemon: ERROR: open %s failed: %d\n",
CONFIG_EXAMPLES_LIBCANARDV1_DEV, errno);
errval = 2;
goto errout_with_dev;
}
/* libcancl functions */
/* Dynamic NodeId */
/* Init UAVCAN register interfaces */
uavcan_node_GetInfo_Response_1_0 node_information; // TODO ADD INFO
uavcan_register_interface_init(&ins, &node_information);
uavcan_register_interface_add_entry("uorb.sensor_gps.0", set_gps_uorb_port_id, get_gps_uorb_port_id);
uavcan_register_interface_add_entry("gnss_fix", set_gps_fix_port_id, get_gps_fix_port_id);
uavcan_register_interface_add_entry("gnss_aux", set_gps_aux_port_id, get_gps_aux_port_id);
initPNPAllocatee(&ins, unique_id);
uint32_t random_no;
random_no = ((float)rand() / RAND_MAX) * (1000000);
uint64_t next_alloc_req = getMonotonicTimestampUSec() + random_no;
while (ins.node_id == CANARD_NODE_ID_UNSET) {
// process the TX and RX buffer
processTxRxOnce(&ins, &sock_ins, 10); //10Ms
const uint64_t ts = getMonotonicTimestampUSec();
if (ts >= next_alloc_req) {
next_alloc_req += ((float)rand() / RAND_MAX) * (1000000);
int32_t result = PNPAllocRequest(&ins);
if (result) {
ins.node_id = PNPGetNodeID();
}
}
}
printf("canard_daemon: canard initialized\n");
printf("start node (ID: %d MTU: %d)\n", ins.node_id,
ins.mtu_bytes);
/* Initialize uORB publishers & subscribers */
uorbConverterInit(&ins, &gps_uorb_port_id, &gps_fix_port_id, &gps_aux_port_id);
g_canard_daemon_started = true;
uint64_t next_1hz_service_at = getMonotonicTimestampUSec();
for (;;) {
processTxRxOnce(&ins, &sock_ins, 10);
uorbProcessSub(10);
}
errout_with_dev:
g_canard_daemon_started = false;
printf("canard_daemon: Terminating!\n");
fflush(stdout);
return errval;
}
/****************************************************************************
* Name: canard_main
*
* Description:
*
****************************************************************************/
int uavcannode_gps_demo_main(int argc, FAR char *argv[])
{
int ret;
printf("canard_main: Starting canard_daemon\n");
if (g_canard_daemon_started) {
printf("canard_main: receive and send task already running\n");
return EXIT_SUCCESS;
}
ret = task_create("canard_daemon", CONFIG_EXAMPLES_LIBCANARDV1_DAEMON_PRIORITY,
CONFIG_EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE, canard_daemon,
argv);
if (ret < 0) {
int errcode = errno;
printf("canard_main: ERROR: Failed to start canard_daemon: %d\n",
errcode);
return EXIT_FAILURE;
}
printf("canard_main: canard_daemon started\n");
return EXIT_SUCCESS;
}

1
src/drivers/uavcannode_gps_demo/libcanard

@ -1 +0,0 @@ @@ -1 +0,0 @@
Subproject commit 2a116170285fb47fcaae150ad21c2ccde0756a5f

12
src/drivers/uavcannode_gps_demo/libcancl/README.md
Unescape View File

@ -1,12 +0,0 @@ @@ -1,12 +0,0 @@
# libcancl
Libcanard client library, that implements common client functionality for use with UAVCAN/CAN
Current features are
- UAVCAN PNP client
- UAVCAN Register interface
- UAVCAN GetInfo responder
# TODO's
- UAVCAN ExecuteCommand implementation
- UAVCAN diagnostic abstraction/helper

264
src/drivers/uavcannode_gps_demo/libcancl/pnp.c
Unescape View File

@ -1,264 +0,0 @@ @@ -1,264 +0,0 @@
/****************************************************************************
* nxp_bms/BMS_v1/src/UAVCAN/pnp.c
*
* BSD 3-Clause License
*
* Copyright 2020 NXP
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "pnp.h"
// Use NuttX crc64 function TODO fallback header for other platforms
#include <crc64.h>
#include "uavcan/node/ID_1_0.h"
#include "uavcan/pnp/NodeIDAllocationData_1_0.h"
#include "uavcan/pnp/NodeIDAllocationData_2_0.h"
#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
#define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
#define PNP_UNIQUE_ID_SIZE 16 // 128 bit unique id
#define PNP_NODE_ID_NO_PREFERENCE CANARD_NODE_ID_MAX - 2 // No preference -> highest possible node ID, note: the two highest node-ID values are reserved for network maintenance tools
/****************************************************************************
* private data
****************************************************************************/
CanardRxSubscription allocSub;
// 128 bit unique id
uint8_t local_unique_id[PNP_UNIQUE_ID_SIZE];
const CanardNodeID preffered_node_id = PNP_NODE_ID_NO_PREFERENCE;
CanardNodeID node_id = CANARD_NODE_ID_UNSET;
CanardTransferID node_id_alloc_transfer_id = 0;
/****************************************************************************
* private Functions declerations
****************************************************************************/
uint64_t makePseudoUniqueId(uint8_t *pUniqueID)
{
// NuttX CRC64/WE implementation
return crc64(pUniqueID, PNP_UNIQUE_ID_SIZE);
}
/****************************************************************************
* public functions
****************************************************************************/
/* Rule A. On initialization:
* 1. The allocatee subscribes to this message.
* 2. The allocatee starts the Request Timer with a random interval [0, 1) sec of Trequest.
*/
uint32_t initPNPAllocatee(CanardInstance *ins, uint8_t *unique_id)
{
// Store unique_id locally
memcpy(&local_unique_id[0], &unique_id[0], sizeof(local_unique_id));
// Create RX Subscriber so we can listen to NodeIDAllocationData msgs
(void) canardRxSubscribe(ins, // Subscribe to messages uavcan.node.Heartbeat.
CanardTransferKindMessage,
(ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID
(ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&allocSub);
// Callee should start a timer with a random interval between 0 and 1 sec
}
/* Rule B. On expiration of the Request Timer (started as per rules A, B, or C):
* 1. Request Timer restarts with a random interval [0, 1) sec of Trequest (chosen anew).
* 2. The allocatee broadcasts an allocation request message, where the fields are populated as follows:
* unique_id_hash - a 48-bit hash of the unique-ID of the allocatee.
* allocated_node_id - empty (not populated).
*/
int32_t PNPAllocRequest(CanardInstance *ins)
{
int32_t result;
CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 10;
// Callee should restart timer
// Allocation already done, nothing to do TODO maybe stop subscribing
if (node_id != CANARD_NODE_ID_UNSET) {
return 1;
}
if (ins->mtu_bytes == CANARD_MTU_CAN_FD) {
// NodeIDAllocationData message
uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg;
uint8_t node_id_alloc_payload_buffer[PNP2_PAYLOAD_SIZE];
memcpy(node_id_alloc_msg.unique_id, local_unique_id, sizeof(node_id_alloc_msg.unique_id));
node_id_alloc_msg.node_id.value = preffered_node_id;
CanardTransfer transfer = {
.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = PNP2_PORT_ID, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = node_id_alloc_transfer_id,
.payload_size = PNP2_PAYLOAD_SIZE,
.payload = &node_id_alloc_payload_buffer,
};
result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, &node_id_alloc_payload_buffer,
&transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(ins, &transfer);
}
} else {
// NodeIDAllocationData message
uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg;
uavcan_pnp_NodeIDAllocationData_1_0_initialize_(&node_id_alloc_msg);
uint8_t node_id_alloc_payload_buffer[PNP1_PAYLOAD_SIZE];
node_id_alloc_msg.unique_id_hash = (makePseudoUniqueId(local_unique_id) & 0xFFFFFFFFFFFF);
CanardTransfer transfer = {
.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = PNP1_PORT_ID, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = node_id_alloc_transfer_id,
.payload_size = PNP1_PAYLOAD_SIZE,
.payload = &node_id_alloc_payload_buffer,
};
result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, &node_id_alloc_payload_buffer,
&transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(ins, &transfer);
}
}
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if the
// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
return result;
}
return 0;
}
/* Rule C. On any allocation message, even if other rules also match:
* 1. Request Timer restarts with a random interval of Trequest (chosen anew).
*
* Rule D. On an allocation message WHERE (source node-ID is non-anonymous, i.e., regular allocation response)
* AND (the field unique_id_hash matches the allocatee's 48-bit unique-ID hash)
* AND (the field allocated_node_id is populated):
* 1. Request Timer stops.
* 2. The allocatee initializes its node-ID with the received value.
* 3. The allocatee terminates its subscription to allocation messages.
* 4. Exit.
*/
int32_t PNPProcess(CanardInstance *ins, CanardTransfer *transfer)
{
// Allocation already done, nothing to do
if (node_id != CANARD_NODE_ID_UNSET) {
return 1;
}
if (transfer->remote_node_id == CANARD_NODE_ID_UNSET) { // Another request, ignore.
return 0;
}
int32_t allocated = CANARD_NODE_ID_UNSET;
if (ins->mtu_bytes == CANARD_MTU_CAN_FD) {
uavcan_pnp_NodeIDAllocationData_2_0 msg;
size_t pnp_in_size_bits = transfer->payload_size;
uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, transfer->payload, &pnp_in_size_bits);
if (memcmp(msg.unique_id, local_unique_id, sizeof(msg.unique_id)) == 0) {
allocated = msg.node_id.value;
}
} else {
uavcan_pnp_NodeIDAllocationData_1_0 msg;
size_t pnp_in_size_bits = transfer->payload_size;
uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, transfer->payload, &pnp_in_size_bits);
if (msg.allocated_node_id.count > 0) {
if (msg.unique_id_hash == (makePseudoUniqueId(local_unique_id) & 0xFFFFFFFFFFFF)) {
allocated = msg.allocated_node_id.elements[0].value;
}
}
}
if (allocated == CANARD_NODE_ID_UNSET) {
return -1; // UID mismatch.
}
if (allocated <= 0 || allocated >= CANARD_NODE_ID_MAX)
// Allocated node-ID ignored because it exceeds max_node_id
{
return -1;
}
// Plug-and-play allocation done: got node-ID %s from server %s', allocated, meta.source_node_id)
node_id = allocated;
return 1;
}
CanardNodeID PNPGetNodeID()
{
return node_id;
}
CanardPortID PNPGetPortID(CanardInstance *ins)
{
return (ins->mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID);
}

74
src/drivers/uavcannode_gps_demo/libcancl/pnp.h
Unescape View File

@ -1,74 +0,0 @@ @@ -1,74 +0,0 @@
/****************************************************************************
* nxp_bms/BMS_v1/inc/UAVCAN/pnp.h
*
* BSD 3-Clause License
*
* Copyright 2020 NXP
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
** ###################################################################
** Filename : pnp.h
** Project : SmartBattery_RDDRONE_BMS772
** Processor : S32K144
** Version : 1.00
** Date : 2020-10-29
** Abstract :
** pnp module.
**
** ###################################################################*/
/*!
** @file pnp.h
**
** @version 01.00
**
** @brief
** pnp module. this module implements the UAVCAN plug and play protocol
**
*/
#ifndef UAVCAN_PNP_H_
#define UAVCAN_PNP_H_
#define NUNAVUT_ASSERT
#include <canard.h>
#include "time.h"
uint32_t initPNPAllocatee(CanardInstance *ins, uint8_t *unique_id);
int32_t PNPAllocRequest(CanardInstance *ins);
int32_t PNPProcess(CanardInstance *ins, CanardTransfer *transfer);
CanardNodeID PNPGetNodeID();
const CanardPortID PNPGetPortID();
#endif //UAVCAN_PNP_H_

282
src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c
Unescape View File

@ -1,282 +0,0 @@ @@ -1,282 +0,0 @@
#include "registerinterface.h"
#include "uavcan/_register/Access_1_0.h"
#include "uavcan/_register/List_1_0.h"
#include "uavcan/_register/Name_1_0.h"
#include "uavcan/_register/Value_1_0.h"
#include <stdio.h>
#include <string.h>
/****************************************************************************
* private data
****************************************************************************/
uavcan_node_GetInfo_Response_1_0 *node_info;
CanardTransferID getinfo_response_transfer_id = 0;
CanardTransferID register_access_response_transfer_id = 0;
CanardTransferID register_list_response_transfer_id = 0;
CanardRxSubscription getinfo_subscription;
CanardRxSubscription register_access_subscription;
CanardRxSubscription register_list_subscription;
//TODO register list and data
uavcan_register_interface_entry register_list[UAVCAN_REGISTER_COUNT];
uint32_t register_list_size = 0;
/****************************************************************************
* public functions
****************************************************************************/
int32_t uavcan_register_interface_init(CanardInstance *ins, uavcan_node_GetInfo_Response_1_0 *info)
{
node_info = info; //TODO think about retention, copy isntead?
(void) canardRxSubscribe(ins,
CanardTransferKindRequest,
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&getinfo_subscription);
(void) canardRxSubscribe(ins,
CanardTransferKindRequest,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&register_access_subscription);
(void) canardRxSubscribe(ins,
CanardTransferKindRequest,
uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&register_list_subscription);
}
int32_t uavcan_register_interface_add_entry(const char *name, register_access_set_callback cb_set,
register_access_get_callback cb_get)
{
if (register_list_size < UAVCAN_REGISTER_COUNT) {
register_list[register_list_size].name = name;
register_list[register_list_size].cb_set = cb_set;
register_list[register_list_size].cb_get = cb_get;
register_list_size++;
return 0;
} else {
return -UAVCAN_REGISTER_ERROR_OUT_OF_MEMORY; // register list full
}
}
// Handler for all PortID registration related messages
int32_t uavcan_register_interface_process(CanardInstance *ins, CanardTransfer *transfer)
{
if (transfer->port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) {
return uavcan_register_interface_get_info_response(ins, transfer);
} else if (transfer->port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
return uavcan_register_interface_access_response(ins, transfer);
} else if (transfer->port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
return uavcan_register_interface_list_response(ins, transfer);
}
return 0; // Nothing to do
}
// Handler for node.GetInfo which yields a response
int32_t uavcan_register_interface_get_info_response(CanardInstance *ins, CanardTransfer *request)
{
uavcan_node_GetInfo_Request_1_0 msg;
size_t in_size_bits;
if (uavcan_node_GetInfo_Request_1_0_deserialize_(&msg, request->payload, request->payload_size) < 0) {
//Error deserialize failed
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
//Note so technically the uavcan_node_GetInfo_Request_1_0 is an empty message not sure if the code above is required
// Setup node.GetInfo response
uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100;
CanardTransfer response = {
.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindResponse,
.port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = request->remote_node_id, // Send back to request Node
.transfer_id = getinfo_response_transfer_id,
.payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
};
int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(node_info, &response_payload_buffer,
&response.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++getinfo_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(ins, &response);
}
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if the
// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
return 1;
}
// Handler for register access interface
int32_t uavcan_register_interface_access_response(CanardInstance *ins, CanardTransfer *request)
{
int index;
{
uavcan_register_Access_Request_1_0 msg;
if (uavcan_register_Access_Request_1_0_deserialize_(&msg, request->payload, &request->payload_size) < 0) {
//Error deserialize failed
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
{
char register_string[255];
int reg_str_len;
for (index = 0; index < register_list_size; index++) {
reg_str_len = sprintf(register_string, "uavcan.pub.%s.id",
register_list[index].name); //TODO more option then pub (sub rate etc)
if (strncmp(msg.name.name.elements, register_string, reg_str_len) == 0) {
if (msg.value._tag_ != 0) { // Value has been set thus we call set handler
if (register_list[index].cb_set(&msg.value) != 0) {
// TODO error ocurred check doc for correct response
}
}
break; // We're done exit loop
}
}
}
}
uavcan_register_Access_Response_1_0 response_msg;
uavcan_register_Access_Response_1_0_initialize_(&response_msg);
if (index < register_list_size) { // Index is available
response_msg.value = register_list[index].cb_get();
} else { // Index not found return empty response
uavcan_register_Value_1_0_initialize_(&response_msg.value);
uavcan_register_Value_1_0_select_empty_(&response_msg.value);
}
uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100;
CanardTransfer response = {
.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindResponse,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = request->remote_node_id, // Send back to request Node
.transfer_id = register_list_response_transfer_id,
.payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
};
int32_t result = uavcan_register_Access_Response_1_0_serialize_(&response_msg, &response_payload_buffer,
&response.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++register_access_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(ins, &response);
}
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if the
// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
return 1;
}
// Handler for register list interface
int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTransfer *request)
{
uavcan_register_List_Request_1_0 msg;
if (uavcan_register_List_Request_1_0_deserialize_(&msg, request->payload, &request->payload_size) < 0) {
//Error deserialize failed
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
//Setup register response
uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; //TODO we know already how big our response is, don't overallocate.
CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + 1000 * 100;
uavcan_register_List_Response_1_0 response_msg;
// Reponse magic start
if (msg.index < register_list_size) {
response_msg.name.name.count = sprintf(response_msg.name.name.elements,
"uavcan.pub.%s.id",
register_list[msg.index].name);
} else {
response_msg.name.name.count = 0;
}
//TODO more option then pub (sub rate
// Response magic end
CanardTransfer response = {
.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindResponse,
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = request->remote_node_id, // Send back to request Node
.transfer_id = register_list_response_transfer_id,
.payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, //See prev TODO
.payload = &response_payload_buffer,
};
int32_t result = uavcan_register_List_Response_1_0_serialize_(&response_msg, &response_payload_buffer,
&response.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++register_list_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(ins, &response);
}
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if the
// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
return -UAVCAN_REGISTER_ERROR_SERIALIZATION;
}
return 1;
}

49
src/drivers/uavcannode_gps_demo/libcancl/registerinterface.h
Unescape View File

@ -1,49 +0,0 @@ @@ -1,49 +0,0 @@
#ifndef UAVCAN_REGISTER_INTERFACE_H_
#define UAVCAN_REGISTER_INTERFACE_H_
#include <canard.h>
#define NUNAVUT_ASSERT
#include "uavcan/node/GetInfo_1_0.h"
#include "uavcan/_register/Value_1_0.h"
#include "time.h"
//No of pre allocated register entries
#ifndef UAVCAN_REGISTER_COUNT
# define UAVCAN_REGISTER_COUNT 5
#endif
#define UAVCAN_REGISTER_ERROR_SERIALIZATION 1
#define UAVCAN_REGISTER_ERROR_OUT_OF_MEMORY 2
typedef int32_t (*register_access_set_callback)(uavcan_register_Value_1_0 *value);
typedef uavcan_register_Value_1_0(*register_access_get_callback)(void);
typedef struct {
/// uavcan.register.Name.1.0 name
const char *name;
register_access_set_callback cb_set;
register_access_get_callback cb_get;
} uavcan_register_interface_entry;
int32_t uavcan_register_interface_init(CanardInstance *ins, uavcan_node_GetInfo_Response_1_0 *info);
int32_t uavcan_register_interface_add_entry(const char *name, register_access_set_callback cb_set,
register_access_get_callback cb_get);
// Handler for all PortID registration related messages
int32_t uavcan_register_interface_process(CanardInstance *ins, CanardTransfer *transfer);
// Handler for node.GetInfo which yields a response
int32_t uavcan_register_interface_get_info_response(CanardInstance *ins, CanardTransfer *request);
// Handler for register access interface
int32_t uavcan_register_interface_access_response(CanardInstance *ins, CanardTransfer *request);
// Handler for register list interface
int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTransfer *request);
#endif //UAVCAN_REGISTER_INTERFACE_H_

6
src/drivers/uavcannode_gps_demo/libcancl/time.h
Unescape View File

@ -1,6 +0,0 @@ @@ -1,6 +0,0 @@
#ifndef MAVCAN_TIME_H_
#define MAVCAN_TIME_H_
uint64_t getMonotonicTimestampUSec(void);
#endif //MAVCAN_TIME_H_

498
src/drivers/uavcannode_gps_demo/o1heap.c
Unescape View File

@ -1,498 +0,0 @@ @@ -1,498 +0,0 @@
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
// documentation files (the "Software"), to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
// and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions
// of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
// Copyright (c) 2020 Pavel Kirienko
// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>
#include "o1heap.h"
#include <assert.h>
// ---------------------------------------- BUILD CONFIGURATION OPTIONS ----------------------------------------
/// The assertion macro defaults to the standard assert().
/// It can be overridden to manually suppress assertion checks or use a different error handling policy.
#ifndef O1HEAP_ASSERT
// Intentional violation of MISRA: the assertion check macro cannot be replaced with a function definition.
# define O1HEAP_ASSERT(x) assert(x) // NOSONAR
#endif
/// Branch probability annotations are used to improve the worst case execution time (WCET). They are entirely optional.
/// A stock implementation is provided for some well-known compilers; for other compilers it defaults to nothing.
/// If you are using a different compiler, consider overriding this value.
#ifndef O1HEAP_LIKELY
# if defined(__GNUC__) || defined(__clang__) || defined(__CC_ARM)
// Intentional violation of MISRA: branch hinting macro cannot be replaced with a function definition.
# define O1HEAP_LIKELY(x) __builtin_expect((x), 1) // NOSONAR
# else
# define O1HEAP_LIKELY(x) x
# endif
#endif
/// This option is used for testing only. Do not use in production.
#if defined(O1HEAP_EXPOSE_INTERNALS) && O1HEAP_EXPOSE_INTERNALS
# define O1HEAP_PRIVATE
#else
# define O1HEAP_PRIVATE static inline
#endif
// ---------------------------------------- INTERNAL DEFINITIONS ----------------------------------------
#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L)
# error "Unsupported language: ISO C99 or a newer version is required."
#endif
#if __STDC_VERSION__ < 201112L
// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition.
# define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR
# define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR
// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context.
# define _static_assert_gl_impl(a, b) a##b // NOSONAR
#endif
/// The overhead is at most O1HEAP_ALIGNMENT bytes large,
/// then follows the user data which shall keep the next fragment aligned.
#define FRAGMENT_SIZE_MIN (O1HEAP_ALIGNMENT * 2U)
/// This is risky, handle with care: if the allocation amount plus per-fragment overhead exceeds 2**(b-1),
/// where b is the pointer bit width, then ceil(log2(amount)) yields b; then 2**b causes an integer overflow.
/// To avoid this, we put a hard limit on fragment size (which is amount + per-fragment overhead): 2**(b-1)
#define FRAGMENT_SIZE_MAX ((SIZE_MAX >> 1U) + 1U)
/// Normally we should subtract log2(FRAGMENT_SIZE_MIN) but log2 is bulky to compute using the preprocessor only.
/// We will certainly end up with unused bins this way, but it is cheap to ignore.
#define NUM_BINS_MAX (sizeof(size_t) * 8U)
static_assert((O1HEAP_ALIGNMENT & (O1HEAP_ALIGNMENT - 1U)) == 0U, "Not a power of 2");
static_assert((FRAGMENT_SIZE_MIN & (FRAGMENT_SIZE_MIN - 1U)) == 0U, "Not a power of 2");
static_assert((FRAGMENT_SIZE_MAX & (FRAGMENT_SIZE_MAX - 1U)) == 0U, "Not a power of 2");
typedef struct Fragment Fragment;
typedef struct FragmentHeader {
Fragment *next;
Fragment *prev;
size_t size;
bool used;
} FragmentHeader;
static_assert(sizeof(FragmentHeader) <= O1HEAP_ALIGNMENT, "Memory layout error");
struct Fragment {
FragmentHeader header;
// Everything past the header may spill over into the allocatable space. The header survives across alloc/free.
Fragment *next_free; // Next free fragment in the bin; NULL in the last one.
Fragment *prev_free; // Same but points back; NULL in the first one.
};
static_assert(sizeof(Fragment) <= FRAGMENT_SIZE_MIN, "Memory layout error");
struct O1HeapInstance {
Fragment *bins[NUM_BINS_MAX]; ///< Smallest fragments are in the bin at index 0.
size_t nonempty_bin_mask; ///< Bit 1 represents a non-empty bin; bin at index 0 is for the smallest fragments.
O1HeapHook critical_section_enter;
O1HeapHook critical_section_leave;
O1HeapDiagnostics diagnostics;
};
/// The amount of space allocated for the heap instance.
/// Its size is padded up to O1HEAP_ALIGNMENT to ensure correct alignment of the allocation arena that follows.
#define INSTANCE_SIZE_PADDED ((sizeof(O1HeapInstance) + O1HEAP_ALIGNMENT - 1U) & ~(O1HEAP_ALIGNMENT - 1U))
static_assert(INSTANCE_SIZE_PADDED >= sizeof(O1HeapInstance), "Invalid instance footprint computation");
static_assert((INSTANCE_SIZE_PADDED % O1HEAP_ALIGNMENT) == 0U, "Invalid instance footprint computation");
/// True if the argument is an integer power of two or zero.
O1HEAP_PRIVATE bool isPowerOf2(const size_t x);
O1HEAP_PRIVATE bool isPowerOf2(const size_t x)
{
return (x & (x - 1U)) == 0U;
}
/// Special case: if the argument is zero, returns zero.
O1HEAP_PRIVATE uint8_t log2Floor(const size_t x);
O1HEAP_PRIVATE uint8_t log2Floor(const size_t x)
{
size_t tmp = x;
uint8_t y = 0;
// This is currently the only exception to the statement "routines contain neither loops nor recursion".
// It is unclear if there is a better way to compute the binary logarithm than this.
while (tmp > 1U) {
tmp >>= 1U;
y++;
}
return y;
}
/// Special case: if the argument is zero, returns zero.
O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x);
O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x)
{
return (uint8_t)(log2Floor(x) + (isPowerOf2(x) ? 0U : 1U));
}
/// Raise 2 into the specified power.
/// You might be tempted to do something like (1U << power). WRONG! We humans are prone to forgetting things.
/// If you forget to cast your 1U to size_t or ULL, you may end up with undefined behavior.
O1HEAP_PRIVATE size_t pow2(const uint8_t power);
O1HEAP_PRIVATE size_t pow2(const uint8_t power)
{
return ((size_t) 1U) << power;
}
O1HEAP_PRIVATE void invoke(const O1HeapHook hook);
O1HEAP_PRIVATE void invoke(const O1HeapHook hook)
{
if (hook != NULL) {
hook();
}
}
/// Links two fragments so that their next/prev pointers point to each other; left goes before right.
O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right);
O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right)
{
if (O1HEAP_LIKELY(left != NULL)) {
left->header.next = right;
}
if (O1HEAP_LIKELY(right != NULL)) {
right->header.prev = left;
}
}
/// Adds a new block into the appropriate bin and updates the lookup mask.
O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment);
O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment)
{
O1HEAP_ASSERT(handle != NULL);
O1HEAP_ASSERT(fragment != NULL);
O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN);
O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U);
const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN); // Round DOWN when inserting.
O1HEAP_ASSERT(idx < NUM_BINS_MAX);
// Add the new fragment to the beginning of the bin list.
// I.e., each allocation will be returning the least-recently-used fragment -- good for caching.
fragment->next_free = handle->bins[idx];
fragment->prev_free = NULL;
if (O1HEAP_LIKELY(handle->bins[idx] != NULL)) {
handle->bins[idx]->prev_free = fragment;
}
handle->bins[idx] = fragment;
handle->nonempty_bin_mask |= pow2(idx);
}
/// Removes the specified block from its bin.
O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment);
O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment)
{
O1HEAP_ASSERT(handle != NULL);
O1HEAP_ASSERT(fragment != NULL);
O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN);
O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U);
const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN); // Round DOWN when removing.
O1HEAP_ASSERT(idx < NUM_BINS_MAX);
// Remove the bin from the free fragment list.
if (O1HEAP_LIKELY(fragment->next_free != NULL)) {
fragment->next_free->prev_free = fragment->prev_free;
}
if (O1HEAP_LIKELY(fragment->prev_free != NULL)) {
fragment->prev_free->next_free = fragment->next_free;
}
// Update the bin header.
if (O1HEAP_LIKELY(handle->bins[idx] == fragment)) {
O1HEAP_ASSERT(fragment->prev_free == NULL);
handle->bins[idx] = fragment->next_free;
if (O1HEAP_LIKELY(handle->bins[idx] == NULL)) {
handle->nonempty_bin_mask &= ~pow2(idx);
}
}
}
// ---------------------------------------- PUBLIC API IMPLEMENTATION ----------------------------------------
O1HeapInstance *o1heapInit(void *const base,
const size_t size,
const O1HeapHook critical_section_enter,
const O1HeapHook critical_section_leave)
{
O1HeapInstance *out = NULL;
if ((base != NULL) && ((((size_t) base) % O1HEAP_ALIGNMENT) == 0U) &&
(size >= (INSTANCE_SIZE_PADDED + FRAGMENT_SIZE_MIN))) {
// Allocate the core heap metadata structure in the beginning of the arena.
O1HEAP_ASSERT(((size_t) base) % sizeof(O1HeapInstance *) == 0U);
out = (O1HeapInstance *) base;
out->nonempty_bin_mask = 0U;
out->critical_section_enter = critical_section_enter;
out->critical_section_leave = critical_section_leave;
for (size_t i = 0; i < NUM_BINS_MAX; i++) {
out->bins[i] = NULL;
}
// Limit and align the capacity.
size_t capacity = size - INSTANCE_SIZE_PADDED;
if (capacity > FRAGMENT_SIZE_MAX) {
capacity = FRAGMENT_SIZE_MAX;
}
while ((capacity % FRAGMENT_SIZE_MIN) != 0) {
O1HEAP_ASSERT(capacity > 0U);
capacity--;
}
O1HEAP_ASSERT((capacity % FRAGMENT_SIZE_MIN) == 0);
O1HEAP_ASSERT((capacity >= FRAGMENT_SIZE_MIN) && (capacity <= FRAGMENT_SIZE_MAX));
// Initialize the root fragment.
Fragment *const frag = (Fragment *)(void *)(((uint8_t *) base) + INSTANCE_SIZE_PADDED);
O1HEAP_ASSERT((((size_t) frag) % O1HEAP_ALIGNMENT) == 0U);
frag->header.next = NULL;
frag->header.prev = NULL;
frag->header.size = capacity;
frag->header.used = false;
frag->next_free = NULL;
frag->prev_free = NULL;
rebin(out, frag);
O1HEAP_ASSERT(out->nonempty_bin_mask != 0U);
// Initialize the diagnostics.
out->diagnostics.capacity = capacity;
out->diagnostics.allocated = 0U;
out->diagnostics.peak_allocated = 0U;
out->diagnostics.peak_request_size = 0U;
out->diagnostics.oom_count = 0U;
}
return out;
}
void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount)
{
O1HEAP_ASSERT(handle != NULL);
O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX);
void *out = NULL;
// If the amount approaches approx. SIZE_MAX/2, an undetected integer overflow may occur.
// To avoid that, we do not attempt allocation if the amount exceeds the hard limit.
// We perform multiple redundant checks to account for a possible unaccounted overflow.
if (O1HEAP_LIKELY((amount > 0U) && (amount <= (handle->diagnostics.capacity - O1HEAP_ALIGNMENT)))) {
// Add the header size and align the allocation size to the power of 2.
// See "Timing-Predictable Memory Allocation In Hard Real-Time Systems", Herter, page 27.
const size_t fragment_size = pow2(log2Ceil(amount + O1HEAP_ALIGNMENT));
O1HEAP_ASSERT(fragment_size <= FRAGMENT_SIZE_MAX);
O1HEAP_ASSERT(fragment_size >= FRAGMENT_SIZE_MIN);
O1HEAP_ASSERT(fragment_size >= amount + O1HEAP_ALIGNMENT);
O1HEAP_ASSERT(isPowerOf2(fragment_size));
const uint8_t optimal_bin_index = log2Ceil(fragment_size / FRAGMENT_SIZE_MIN); // Use CEIL when fetching.
O1HEAP_ASSERT(optimal_bin_index < NUM_BINS_MAX);
const size_t candidate_bin_mask = ~(pow2(optimal_bin_index) - 1U);
invoke(handle->critical_section_enter);
// Find the smallest non-empty bin we can use.
const size_t suitable_bins = handle->nonempty_bin_mask & candidate_bin_mask;
const size_t smallest_bin_mask = suitable_bins & ~(suitable_bins - 1U); // Clear all bits but the lowest.
if (O1HEAP_LIKELY(smallest_bin_mask != 0)) {
O1HEAP_ASSERT(isPowerOf2(smallest_bin_mask));
const uint8_t bin_index = log2Floor(smallest_bin_mask);
O1HEAP_ASSERT(bin_index >= optimal_bin_index);
O1HEAP_ASSERT(bin_index < NUM_BINS_MAX);
// The bin we found shall not be empty, otherwise it's a state divergence (memory corruption?).
Fragment *const frag = handle->bins[bin_index];
O1HEAP_ASSERT(frag != NULL);
O1HEAP_ASSERT(frag->header.size >= fragment_size);
O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U);
O1HEAP_ASSERT(!frag->header.used);
unbin(handle, frag);
// Split the fragment if it is too large.
const size_t leftover = frag->header.size - fragment_size;
frag->header.size = fragment_size;
O1HEAP_ASSERT(leftover < handle->diagnostics.capacity); // Overflow check.
O1HEAP_ASSERT(leftover % FRAGMENT_SIZE_MIN == 0U); // Alignment check.
if (O1HEAP_LIKELY(leftover >= FRAGMENT_SIZE_MIN)) {
Fragment *const new_frag = (Fragment *)(void *)(((uint8_t *) frag) + fragment_size);
O1HEAP_ASSERT(((size_t) new_frag) % O1HEAP_ALIGNMENT == 0U);
new_frag->header.size = leftover;
new_frag->header.used = false;
interlink(new_frag, frag->header.next);
interlink(frag, new_frag);
rebin(handle, new_frag);
}
// Update the diagnostics.
O1HEAP_ASSERT((handle->diagnostics.allocated % FRAGMENT_SIZE_MIN) == 0U);
handle->diagnostics.allocated += fragment_size;
O1HEAP_ASSERT(handle->diagnostics.allocated <= handle->diagnostics.capacity);
if (O1HEAP_LIKELY(handle->diagnostics.peak_allocated < handle->diagnostics.allocated)) {
handle->diagnostics.peak_allocated = handle->diagnostics.allocated;
}
// Finalize the fragment we just allocated.
O1HEAP_ASSERT(frag->header.size >= amount + O1HEAP_ALIGNMENT);
frag->header.used = true;
out = ((uint8_t *) frag) + O1HEAP_ALIGNMENT;
}
} else {
invoke(handle->critical_section_enter);
}
// Update the diagnostics.
if (O1HEAP_LIKELY(handle->diagnostics.peak_request_size < amount)) {
handle->diagnostics.peak_request_size = amount;
}
if (O1HEAP_LIKELY((out == NULL) && (amount > 0U))) {
handle->diagnostics.oom_count++;
}
invoke(handle->critical_section_leave);
return out;
}
void o1heapFree(O1HeapInstance *const handle, void *const pointer)
{
O1HEAP_ASSERT(handle != NULL);
O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX);
if (O1HEAP_LIKELY(pointer != NULL)) { // NULL pointer is a no-op.
Fragment *const frag = (Fragment *)(void *)(((uint8_t *) pointer) - O1HEAP_ALIGNMENT);
// Check for heap corruption in debug builds.
O1HEAP_ASSERT(((size_t) frag) % sizeof(Fragment *) == 0U);
O1HEAP_ASSERT(((size_t) frag) >= (((size_t) handle) + INSTANCE_SIZE_PADDED));
O1HEAP_ASSERT(((size_t) frag) <=
(((size_t) handle) + INSTANCE_SIZE_PADDED + handle->diagnostics.capacity - FRAGMENT_SIZE_MIN));
O1HEAP_ASSERT(frag->header.used); // Catch double-free
O1HEAP_ASSERT(((size_t) frag->header.next) % sizeof(Fragment *) == 0U);
O1HEAP_ASSERT(((size_t) frag->header.prev) % sizeof(Fragment *) == 0U);
O1HEAP_ASSERT(frag->header.size >= FRAGMENT_SIZE_MIN);
O1HEAP_ASSERT(frag->header.size <= handle->diagnostics.capacity);
O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U);
invoke(handle->critical_section_enter);
// Even if we're going to drop the fragment later, mark it free anyway to prevent double-free.
frag->header.used = false;
// Update the diagnostics. It must be done before merging because it invalidates the fragment size information.
O1HEAP_ASSERT(handle->diagnostics.allocated >= frag->header.size); // Heap corruption check.
handle->diagnostics.allocated -= frag->header.size;
// Merge with siblings and insert the returned fragment into the appropriate bin and update metadata.
Fragment *const prev = frag->header.prev;
Fragment *const next = frag->header.next;
const bool join_left = (prev != NULL) && (!prev->header.used);
const bool join_right = (next != NULL) && (!next->header.used);
if (join_left && join_right) { // [ prev ][ this ][ next ] => [ ------- prev ------- ]
unbin(handle, prev);
unbin(handle, next);
prev->header.size += frag->header.size + next->header.size;
frag->header.size = 0; // Invalidate the dropped fragment headers to prevent double-free.
next->header.size = 0;
O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U);
interlink(prev, next->header.next);
rebin(handle, prev);
} else if (join_left) { // [ prev ][ this ][ next ] => [ --- prev --- ][ next ]
unbin(handle, prev);
prev->header.size += frag->header.size;
frag->header.size = 0;
O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U);
interlink(prev, next);
rebin(handle, prev);
} else if (join_right) { // [ prev ][ this ][ next ] => [ prev ][ --- this --- ]
unbin(handle, next);
frag->header.size += next->header.size;
next->header.size = 0;
O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U);
interlink(frag, next->header.next);
rebin(handle, frag);
} else {
rebin(handle, frag);
}
invoke(handle->critical_section_leave);
}
}
bool o1heapDoInvariantsHold(const O1HeapInstance *const handle)
{
O1HEAP_ASSERT(handle != NULL);
bool valid = true;
invoke(handle->critical_section_enter);
// Check the bin mask consistency.
for (size_t i = 0; i < NUM_BINS_MAX; i++) { // Dear compiler, feel free to unroll this loop.
const bool mask_bit_set = (handle->nonempty_bin_mask & pow2((uint8_t) i)) != 0U;
const bool bin_nonempty = handle->bins[i] != NULL;
valid = valid && (mask_bit_set == bin_nonempty);
}
// Create a local copy of the diagnostics struct to check later and release the critical section early.
const O1HeapDiagnostics diag = handle->diagnostics;
invoke(handle->critical_section_leave);
// Capacity check.
valid = valid && (diag.capacity <= FRAGMENT_SIZE_MAX) && (diag.capacity >= FRAGMENT_SIZE_MIN) &&
((diag.capacity % FRAGMENT_SIZE_MIN) == 0U);
// Allocation info check.
valid = valid && (diag.allocated <= diag.capacity) && ((diag.allocated % FRAGMENT_SIZE_MIN) == 0U) &&
(diag.peak_allocated <= diag.capacity) && (diag.peak_allocated >= diag.allocated) &&
((diag.peak_allocated % FRAGMENT_SIZE_MIN) == 0U);
// Peak request check
valid = valid && ((diag.peak_request_size < diag.capacity) || (diag.oom_count > 0U));
if (diag.peak_request_size == 0U) {
valid = valid && (diag.peak_allocated == 0U) && (diag.allocated == 0U) && (diag.oom_count == 0U);
} else {
valid = valid && // Overflow on summation is possible but safe to ignore.
(((diag.peak_request_size + O1HEAP_ALIGNMENT) <= diag.peak_allocated) || (diag.oom_count > 0U));
}
return valid;
}
O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle)
{
O1HEAP_ASSERT(handle != NULL);
invoke(handle->critical_section_enter);
const O1HeapDiagnostics out = handle->diagnostics;
invoke(handle->critical_section_leave);
return out;
}

142
src/drivers/uavcannode_gps_demo/o1heap.h
Unescape View File

@ -1,142 +0,0 @@ @@ -1,142 +0,0 @@
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
// documentation files (the "Software"), to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
// and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions
// of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
// Copyright (c) 2020 Pavel Kirienko
// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>
//
// READ THE DOCUMENTATION IN README.md.
#ifndef O1HEAP_H_INCLUDED
#define O1HEAP_H_INCLUDED
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
/// The semantic version number of this distribution.
#define O1HEAP_VERSION_MAJOR 1
/// The guaranteed alignment depends on the platform pointer width.
#define O1HEAP_ALIGNMENT (sizeof(void*) * 4U)
/// The definition is private, so the user code can only operate on pointers. This is done to enforce encapsulation.
typedef struct O1HeapInstance O1HeapInstance;
/// A hook function invoked by the allocator. NULL hooks are silently not invoked (not an error).
typedef void (*O1HeapHook)(void);
/// Runtime diagnostic information. This information can be used to facilitate runtime self-testing,
/// as required by certain safety-critical development guidelines.
/// If assertion checks are not disabled, the library will perform automatic runtime self-diagnostics that trigger
/// an assertion failure if a heap corruption is detected.
/// Health checks and validation can be done with @ref o1heapDoInvariantsHold().
typedef struct {
/// The total amount of memory available for serving allocation requests (heap size).
/// The maximum allocation size is (capacity - O1HEAP_ALIGNMENT).
/// This parameter does not include the overhead used up by @ref O1HeapInstance and arena alignment.
/// This parameter is constant.
size_t capacity;
/// The amount of memory that is currently allocated, including the per-fragment overhead and size alignment.
/// For example, if the application requested a fragment of size 1 byte, the value reported here may be 32 bytes.
size_t allocated;
/// The maximum value of 'allocated' seen since initialization. This parameter is never decreased.
size_t peak_allocated;
/// The largest amount of memory that the allocator has attempted to allocate (perhaps unsuccessfully)
/// since initialization (not including the rounding and the allocator's own per-fragment overhead,
/// so the total is larger). This parameter is never decreased. The initial value is zero.
size_t peak_request_size;
/// The number of times an allocation request could not be completed due to the lack of memory or
/// excessive fragmentation. OOM stands for "out of memory". This parameter is never decreased.
uint64_t oom_count;
} O1HeapDiagnostics;
/// The arena base pointer shall be aligned at @ref O1HEAP_ALIGNMENT, otherwise NULL is returned.
///
/// The total heap capacity cannot exceed approx. (SIZE_MAX/2). If the arena size allows for a larger heap,
/// the excess will be silently truncated away (no error). This is not a realistic use case because a typical
/// application is unlikely to be able to dedicate that much of the address space for the heap.
///
/// The critical section enter/leave callbacks will be invoked when the allocator performs an atomic transaction.
/// There is at most one atomic transaction per allocation/deallocation.
/// Either or both of the callbacks may be NULL if locking is not needed (i.e., the heap is not shared).
/// It is guaranteed that a critical section will never be entered recursively.
/// It is guaranteed that 'enter' is invoked the same number of times as 'leave', unless either of them are NULL.
/// It is guaranteed that 'enter' is invoked before 'leave', unless either of them are NULL.
/// The callbacks are never invoked from the initialization function itself.
///
/// The function initializes a new heap instance allocated in the provided arena, taking some of its space for its
/// own needs (normally about 40..600 bytes depending on the architecture, but this parameter is not characterized).
/// A pointer to the newly initialized instance is returned.
///
/// If the provided space is insufficient, NULL is returned.
///
/// An initialized instance does not hold any resources. Therefore, if the instance is no longer needed,
/// it can be discarded without any de-initialization procedures.
///
/// The time complexity is unspecified.
O1HeapInstance *o1heapInit(void *const base,
const size_t size,
const O1HeapHook critical_section_enter,
const O1HeapHook critical_section_leave);
/// The semantics follows malloc() with additional guarantees the full list of which is provided below.
///
/// If the allocation request is served successfully, a pointer to the newly allocated memory fragment is returned.
/// The returned pointer is guaranteed to be aligned at @ref O1HEAP_ALIGNMENT.
///
/// If the allocation request cannot be served due to the lack of memory or its excessive fragmentation,
/// a NULL pointer is returned.
///
/// The function is executed in constant time (unless the critical section management hooks are used and are not
/// constant-time). The allocated memory is NOT zero-filled (because zero-filling is a variable-complexity operation).
///
/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored).
void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount);
/// The semantics follows free() with additional guarantees the full list of which is provided below.
///
/// If the pointer does not point to a previously allocated block and is not NULL, the behavior is undefined.
/// Builds where assertion checks are enabled may trigger an assertion failure for some invalid inputs.
///
/// The function is executed in constant time (unless the critical section management hooks are used and are not
/// constant-time).
///
/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored).
void o1heapFree(O1HeapInstance *const handle, void *const pointer);
/// Performs a basic sanity check on the heap.
/// This function can be used as a weak but fast method of heap corruption detection.
/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL).
/// If the handle pointer is NULL, the behavior is undefined.
/// The time complexity is constant.
/// The return value is truth if the heap looks valid, falsity otherwise.
bool o1heapDoInvariantsHold(const O1HeapInstance *const handle);
/// Samples and returns a copy of the diagnostic information, see @ref O1HeapDiagnostics.
/// This function merely copies the structure from an internal storage, so it is fast to return.
/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL).
/// If the handle pointer is NULL, the behavior is undefined.
O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle);
#ifdef __cplusplus
}
#endif
#endif // O1HEAP_H_INCLUDED

1
src/drivers/uavcannode_gps_demo/public_regulated_data_types

@ -1 +0,0 @@ @@ -1 +0,0 @@
Subproject commit 0a773b93ce5c94e1d2791b180058cb9897fab7e1

172
src/drivers/uavcannode_gps_demo/socketcan.c
Unescape View File

@ -1,172 +0,0 @@ @@ -1,172 +0,0 @@
#include "socketcan.h"
#include <stdio.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <string.h>
int16_t socketcanOpen(CanardSocketInstance *ins, const char *const can_iface_name, const bool can_fd)
{
struct sockaddr_can addr;
struct ifreq ifr;
ins->can_fd = can_fd;
/* open socket */
if ((ins->s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
perror("socket");
return -1;
}
strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1);
ifr.ifr_name[IFNAMSIZ - 1] = '\0';
ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);
if (!ifr.ifr_ifindex) {
perror("if_nametoindex");
return -1;
}
memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
const int on = 1;
/* RX Timestamping */
if (setsockopt(ins->s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
perror("SO_TIMESTAMP is disabled");
return -1;
}
/* NuttX Feature: Enable TX deadline when sending CAN frames
* When a deadline occurs the driver will remove the CAN frame
*/
if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) {
perror("CAN_RAW_TX_DEADLINE is disabled");
return -1;
}
if (can_fd) {
if (setsockopt(ins->s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) {
perror("no CAN FD support");
return -1;
}
}
if (bind(ins->s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
perror("bind");
return -1;
}
// Setup TX msg
ins->send_iov.iov_base = &ins->send_frame;
if (ins->can_fd) {
ins->send_iov.iov_len = sizeof(struct canfd_frame);
} else {
ins->send_iov.iov_len = sizeof(struct can_frame);
}
memset(&ins->send_control, 0x00, sizeof(ins->send_control));
ins->send_msg.msg_iov = &ins->send_iov;
ins->send_msg.msg_iovlen = 1;
ins->send_msg.msg_control = &ins->send_control;
ins->send_msg.msg_controllen = sizeof(ins->send_control);
ins->send_cmsg = CMSG_FIRSTHDR(&ins->send_msg);
ins->send_cmsg->cmsg_level = SOL_CAN_RAW;
ins->send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE;
ins->send_cmsg->cmsg_len = sizeof(struct timeval);
ins->send_tv = (struct timeval *)CMSG_DATA(&ins->send_cmsg);
// Setup RX msg
ins->recv_iov.iov_base = &ins->recv_frame;
if (can_fd) {
ins->recv_iov.iov_len = sizeof(struct canfd_frame);
} else {
ins->recv_iov.iov_len = sizeof(struct can_frame);
}
memset(&ins->recv_control, 0x00, sizeof(ins->recv_control));
ins->recv_msg.msg_iov = &ins->recv_iov;
ins->recv_msg.msg_iovlen = 1;
ins->recv_msg.msg_control = &ins->recv_control;
ins->recv_msg.msg_controllen = sizeof(ins->recv_control);
ins->recv_cmsg = CMSG_FIRSTHDR(&ins->recv_msg);
return 0;
}
int16_t socketcanTransmit(CanardSocketInstance *ins, const CanardFrame *txf)
{
/* Copy CanardFrame to can_frame/canfd_frame */
if (ins->can_fd) {
ins->send_frame.can_id = txf->extended_can_id;
ins->send_frame.can_id |= CAN_EFF_FLAG;
ins->send_frame.len = txf->payload_size;
memcpy(&ins->send_frame.data, txf->payload, txf->payload_size);
} else {
struct can_frame *frame = (struct can_frame *)&ins->send_frame;
frame->can_id = txf->extended_can_id;
frame->can_id |= CAN_EFF_FLAG;
frame->can_dlc = txf->payload_size;
memcpy(&frame->data, txf->payload, txf->payload_size);
}
/* Set CAN_RAW_TX_DEADLINE timestamp */
ins->send_tv->tv_usec = txf->timestamp_usec % 1000000ULL;
ins->send_tv->tv_sec = (txf->timestamp_usec - ins->send_tv->tv_usec) / 1000000ULL;
return sendmsg(ins->s, &ins->send_msg, 0);
}
int16_t socketcanReceive(CanardSocketInstance *ins, CanardFrame *rxf)
{
int32_t result = recvmsg(ins->s, &ins->recv_msg, 0);
if (result < 0) {
return result;
}
/* Copy CAN frame to CanardFrame */
if (ins->can_fd) {
struct canfd_frame *recv_frame = (struct canfd_frame *)&ins->recv_frame;
rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
rxf->payload_size = recv_frame->len;
rxf->payload = &recv_frame->data;
} else {
struct can_frame *recv_frame = (struct can_frame *)&ins->recv_frame;
rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK;
rxf->payload_size = recv_frame->can_dlc;
rxf->payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference
}
/* Read SO_TIMESTAMP value */
if (ins->recv_cmsg->cmsg_level == SOL_SOCKET && ins->recv_cmsg->cmsg_type == SO_TIMESTAMP) {
struct timeval *tv = (struct timeval *)CMSG_DATA(ins->recv_cmsg);
rxf->timestamp_usec = tv->tv_sec * 1000000ULL + tv->tv_usec;
}
return result;
}
/* TODO implement corresponding IOCTL */
int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters)
{
return -1;
}

69
src/drivers/uavcannode_gps_demo/socketcan.h
Unescape View File

@ -1,69 +0,0 @@ @@ -1,69 +0,0 @@
#ifndef SOCKETCAN_H_INCLUDED
#define SOCKETCAN_H_INCLUDED
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <nuttx/can.h>
#include <netpacket/can.h>
#include <canard.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct CanardSocketInstance CanardSocketInstance;
typedef int fd_t;
struct CanardSocketInstance {
fd_t s;
bool can_fd;
//// Send msg structure
struct iovec send_iov;
struct canfd_frame send_frame;
struct msghdr send_msg;
struct cmsghdr *send_cmsg;
struct timeval *send_tv; /* TX deadline timestamp */
uint8_t send_control[sizeof(struct cmsghdr) + sizeof(struct timeval)];
//// Receive msg structure
struct iovec recv_iov;
struct canfd_frame recv_frame;
struct msghdr recv_msg;
struct cmsghdr *recv_cmsg;
uint8_t recv_control[sizeof(struct cmsghdr) + sizeof(struct timeval)];
};
/// Creates a SocketCAN socket for corresponding iface can_iface_name
/// Also sets up the message structures required for socketcanTransmit & socketcanReceive
/// can_fd determines to use CAN FD frame when is 1, and classical CAN frame when is 0
/// The return value is 0 on succes and -1 on error
int16_t socketcanOpen(CanardSocketInstance *ins, const char *const can_iface_name, const bool can_fd);
/// Send a CanardFrame to the CanardSocketInstance socket
/// This function is blocking
/// The return value is number of bytes transferred, negative value on error.
int16_t socketcanTransmit(CanardSocketInstance *ins, const CanardFrame *txf);
/// Receive a CanardFrame from the CanardSocketInstance socket
/// This function is blocking
/// The return value is number of bytes received, negative value on error.
int16_t socketcanReceive(CanardSocketInstance *ins, CanardFrame *rxf);
// TODO implement ioctl for CAN filter
int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters);
#ifdef __cplusplus
}
#endif
#endif //SOCKETCAN_H_INCLUDED

108
src/drivers/uavcannode_gps_demo/uorb_converter.c
Unescape View File

@ -1,108 +0,0 @@ @@ -1,108 +0,0 @@
/*
* uorb_converter.c
*
* Created on: Apr 10, 2020
* Author: hovergames
*/
#include <inttypes.h>
#include <string.h>
#include "uorb_converter.h"
/****************************************************************************
* Private Data
****************************************************************************/
int uorb_sub_fd;
px4_pollfd_struct_t fds[1];
int error_counter;
CanardInstance *canard_ins;
int raw_uorb_message_transfer_id;
int fix_message_transfer_id;
int aux_message_transfer_id;
int16_t *gps_raw_uorb_port_id;
int16_t *gps_fix_port_id;
int16_t *gps_aux_port_id;
void uorbConverterInit(CanardInstance *ins, int16_t *raw_uorb_port_id, int16_t *fix_port_id, int16_t *aux_port_id)
{
canard_ins = ins;
gps_raw_uorb_port_id = raw_uorb_port_id;
gps_fix_port_id = fix_port_id;
gps_aux_port_id = aux_port_id;
/* subscribe to the uORB topic */
uorb_sub_fd = orb_subscribe(ORB_ID(sensor_gps));
orb_set_interval(uorb_sub_fd, 200);
/* one could wait for multiple topics with this technique, just using one here */
fds[0].fd = uorb_sub_fd;
fds[0].events = POLLIN;
error_counter = 0;
raw_uorb_message_transfer_id = 0;
fix_message_transfer_id = 0;
aux_message_transfer_id = 0;
}
void uorbProcessSub(int timeout_msec)
{
/* wait for subscriber update of 1 file descriptor for timeout_msec ms */
int poll_ret = px4_poll(fds, 1, timeout_msec);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
PX4_ERR("ERROR return value from poll(): %d", poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
/* obtained data for the first file descriptor */
struct sensor_gps_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_gps), uorb_sub_fd, &raw);
CanardMicrosecond transmission_deadline = getMonotonicTimestampUSec() + (uint64_t)(1000ULL * 100ULL);
// raw uORB sensor_gps message
if (*gps_raw_uorb_port_id != -1) {
CanardTransfer transfer = {
.timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = *gps_raw_uorb_port_id, // This is the subject-ID.
.remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET.
.transfer_id = raw_uorb_message_transfer_id,
.payload_size = sizeof(struct sensor_gps_s),
.payload = &raw,
};
++raw_uorb_message_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
int32_t result = canardTxPush(canard_ins, &transfer);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
// It is possible to statically prove that an out-of-memory will never occur for a given application if the
// heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
PX4_ERR("canardTxPush error %" PRId32, result);
}
}
PX4_INFO("Recv from uORB");
}
}
}

34
src/drivers/uavcannode_gps_demo/uorb_converter.h
Unescape View File

@ -1,34 +0,0 @@ @@ -1,34 +0,0 @@
/*
* uorb_converter.h
*
* Created on: Apr 10, 2020
* Author: hovergames
*/
#ifndef SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_
#define SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_
/* uORB */
#include <px4_platform_common/posix.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_gps.h>
/* canard */
#include <canard.h>
#include <canard_dsdl.h>
/* monotonic timestamp */
#include "libcancl/time.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
void uorbConverterInit(CanardInstance *ins, int16_t *raw_uorb_port_id, int16_t *fix_port_id, int16_t *aux_port_id);
void uorbProcessSub(int timeout_msec);
#endif /* SRC_DRIVERS_UAVCANNODE_V1_UORB_CONVERTER_H_ */
Write Preview
Loading…
Cancel
Save