Add libcanardv1 for Cyphal, rename libcanard to libcanardv0
This commit is contained in:
parent
cfa5af51cd
commit
819c34e975
@ -3,22 +3,22 @@
|
|||||||
# see the file kconfig-language.txt in the NuttX tools repository.
|
# see the file kconfig-language.txt in the NuttX tools repository.
|
||||||
#
|
#
|
||||||
|
|
||||||
config CANUTILS_LIBCANARD
|
config CANUTILS_LIBCANARDV0
|
||||||
bool "libcanard UAVCAN Library"
|
bool "libcanard UAVCAN v0 Library"
|
||||||
default n
|
default n
|
||||||
depends on CAN && CAN_EXTID
|
depends on CAN && CAN_EXTID
|
||||||
---help---
|
---help---
|
||||||
Enable the libcanard UAVCAN library.
|
Enable the libcanard UAVCAN v0 library.
|
||||||
|
|
||||||
if CANUTILS_LIBCANARD
|
if CANUTILS_LIBCANARDV0
|
||||||
|
|
||||||
config LIBCANARD_URL
|
config LIBCANARDV0_URL
|
||||||
string "libcanard URL"
|
string "libcanard URL"
|
||||||
default "https://github.com/UAVCAN/libcanard/archive"
|
default "https://github.com/UAVCAN/libcanard/archive"
|
||||||
---help---
|
---help---
|
||||||
libcanard URL.
|
libcanard URL.
|
||||||
|
|
||||||
config LIBCANARD_VERSION
|
config LIBCANARDV0_VERSION
|
||||||
string "libcanard Version"
|
string "libcanard Version"
|
||||||
default "5ad65c6a4efda60cda7a8f0512da0f465822bbb8"
|
default "5ad65c6a4efda60cda7a8f0512da0f465822bbb8"
|
||||||
---help---
|
---help---
|
@ -1,5 +1,5 @@
|
|||||||
############################################################################
|
############################################################################
|
||||||
# apps/canutils/libcanard/Make.defs
|
# apps/canutils/libcanardv0/Make.defs
|
||||||
#
|
#
|
||||||
# Licensed to the Apache Software Foundation (ASF) under one or more
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
# contributor license agreements. See the NOTICE file distributed with
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
@ -18,6 +18,6 @@
|
|||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
ifeq ($(CONFIG_CANUTILS_LIBCANARD),y)
|
ifeq ($(CONFIG_CANUTILS_LIBCANARDV0),y)
|
||||||
CONFIGURED_APPS += $(APPDIR)/canutils/libcanard
|
CONFIGURED_APPS += $(APPDIR)/canutils/libcanardv0
|
||||||
endif
|
endif
|
@ -1,5 +1,5 @@
|
|||||||
############################################################################
|
############################################################################
|
||||||
# apps/canutils/libcanard/Makefile
|
# apps/canutils/libcanardv0/Makefile
|
||||||
#
|
#
|
||||||
# Licensed to the Apache Software Foundation (ASF) under one or more
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
# contributor license agreements. See the NOTICE file distributed with
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
@ -23,38 +23,38 @@ include $(APPDIR)/Make.defs
|
|||||||
UNPACK = unzip
|
UNPACK = unzip
|
||||||
PACKEXT = .zip
|
PACKEXT = .zip
|
||||||
|
|
||||||
LIBCANARD_URL = $(patsubst "%",%,$(strip $(CONFIG_LIBCANARD_URL)))
|
LIBCANARDV0_URL = $(patsubst "%",%,$(strip $(CONFIG_LIBCANARDV0_URL)))
|
||||||
LIBCANARD_VERSION = $(patsubst "%",%,$(strip $(CONFIG_LIBCANARD_VERSION)))
|
LIBCANARDV0_VERSION = $(patsubst "%",%,$(strip $(CONFIG_LIBCANARDV0_VERSION)))
|
||||||
LIBCANARD_UNPACKNAME = libcanard-$(LIBCANARD_VERSION)
|
LIBCANARDV0_UNPACKNAME = libcanard-$(LIBCANARDV0_VERSION)
|
||||||
LIBCANARD_PACKNAME = $(LIBCANARD_UNPACKNAME)$(PACKEXT)
|
LIBCANARDV0_PACKNAME = $(LIBCANARDV0_UNPACKNAME)$(PACKEXT)
|
||||||
LIBCANARD_SRCDIR = $(LIBCANARD_UNPACKNAME)
|
LIBCANARDV0_SRCDIR = $(LIBCANARDV0_UNPACKNAME)
|
||||||
LIBCANARD_DRVDIR = $(LIBCANARD_SRCDIR)$(DELIM)drivers$(DELIM)nuttx
|
LIBCANARDV0_DRVDIR = $(LIBCANARDV0_SRCDIR)$(DELIM)drivers$(DELIM)nuttx
|
||||||
|
|
||||||
APPS_INCDIR = $(APPDIR)$(DELIM)include$(DELIM)canutils
|
APPS_INCDIR = $(APPDIR)$(DELIM)include$(DELIM)canutils
|
||||||
|
|
||||||
CFLAGS += -std=c99 -DCANARD_ASSERT=DEBUGASSERT
|
CFLAGS += -std=c99 -DCANARD_ASSERT=DEBUGASSERT
|
||||||
CFLAGS += ${shell $(INCDIR) "$(CC)" $(APPS_INCDIR)}
|
CFLAGS += ${shell $(INCDIR) "$(CC)" $(APPS_INCDIR)}
|
||||||
|
|
||||||
CSRCS = $(LIBCANARD_SRCDIR)$(DELIM)canard.c $(LIBCANARD_DRVDIR)$(DELIM)canard_nuttx.c
|
CSRCS = $(LIBCANARDV0_SRCDIR)$(DELIM)canard.c $(LIBCANARDV0_DRVDIR)$(DELIM)canard_nuttx.c
|
||||||
|
|
||||||
$(LIBCANARD_PACKNAME):
|
$(LIBCANARDV0_PACKNAME):
|
||||||
@echo "Downloading: $@"
|
@echo "Downloading: $@"
|
||||||
$(Q) curl -o $@ -L $(LIBCANARD_URL)$(DELIM)$(LIBCANARD_VERSION)$(PACKEXT)
|
$(Q) curl -o $@ -L $(LIBCANARDV0_URL)$(DELIM)$(LIBCANARDV0_VERSION)$(PACKEXT)
|
||||||
|
|
||||||
$(LIBCANARD_UNPACKNAME): $(LIBCANARD_PACKNAME)
|
$(LIBCANARDV0_UNPACKNAME): $(LIBCANARDV0_PACKNAME)
|
||||||
@echo "Unpacking: $< -> $@"
|
@echo "Unpacking: $< -> $@"
|
||||||
$(call DELDIR, $@)
|
$(call DELDIR, $@)
|
||||||
$(Q) $(UNPACK) $<
|
$(Q) $(UNPACK) $<
|
||||||
$(Q) touch $@
|
$(Q) touch $@
|
||||||
|
|
||||||
$(LIBCANARD_SRCDIR)$(DELIM)canard.h: $(LIBCANARD_UNPACKNAME)
|
$(LIBCANARDV0_SRCDIR)$(DELIM)canard.h: $(LIBCANARDV0_UNPACKNAME)
|
||||||
|
|
||||||
$(LIBCANARD_DRVDIR)$(DELIM)canard_nuttx.h: $(LIBCANARD_UNPACKNAME)
|
$(LIBCANARDV0_DRVDIR)$(DELIM)canard_nuttx.h: $(LIBCANARDV0_UNPACKNAME)
|
||||||
|
|
||||||
$(APPS_INCDIR)$(DELIM)canard.h: $(LIBCANARD_SRCDIR)$(DELIM)canard.h
|
$(APPS_INCDIR)$(DELIM)canard.h: $(LIBCANARDV0_SRCDIR)$(DELIM)canard.h
|
||||||
$(Q) cp $< $@
|
$(Q) cp $< $@
|
||||||
|
|
||||||
$(APPS_INCDIR)$(DELIM)canard_nuttx.h: $(LIBCANARD_DRVDIR)$(DELIM)canard_nuttx.h
|
$(APPS_INCDIR)$(DELIM)canard_nuttx.h: $(LIBCANARDV0_DRVDIR)$(DELIM)canard_nuttx.h
|
||||||
$(Q) cp $< $@
|
$(Q) cp $< $@
|
||||||
|
|
||||||
context:: $(APPS_INCDIR)$(DELIM)canard.h $(APPS_INCDIR)$(DELIM)canard_nuttx.h
|
context:: $(APPS_INCDIR)$(DELIM)canard.h $(APPS_INCDIR)$(DELIM)canard_nuttx.h
|
||||||
@ -65,7 +65,7 @@ clean::
|
|||||||
distclean::
|
distclean::
|
||||||
$(call DELFILE, $(APPS_INCDIR)$(DELIM)canard.h)
|
$(call DELFILE, $(APPS_INCDIR)$(DELIM)canard.h)
|
||||||
$(call DELFILE, $(APPS_INCDIR)$(DELIM)canard_nuttx.h)
|
$(call DELFILE, $(APPS_INCDIR)$(DELIM)canard_nuttx.h)
|
||||||
$(call DELDIR, $(LIBCANARD_UNPACKNAME))
|
$(call DELDIR, $(LIBCANARDV0_UNPACKNAME))
|
||||||
$(call DELFILE, $(LIBCANARD_PACKNAME))
|
$(call DELFILE, $(LIBCANARDV0_PACKNAME))
|
||||||
|
|
||||||
include $(APPDIR)/Application.mk
|
include $(APPDIR)/Application.mk
|
2
canutils/libcanardv1/.gitignore
vendored
Normal file
2
canutils/libcanardv1/.gitignore
vendored
Normal file
@ -0,0 +1,2 @@
|
|||||||
|
/libcanard-*
|
||||||
|
/o1heap-*
|
39
canutils/libcanardv1/Kconfig
Normal file
39
canutils/libcanardv1/Kconfig
Normal file
@ -0,0 +1,39 @@
|
|||||||
|
#
|
||||||
|
# For a description of the syntax of this configuration file,
|
||||||
|
# see the file kconfig-language.txt in the NuttX tools repository.
|
||||||
|
#
|
||||||
|
|
||||||
|
config CANUTILS_LIBCANARDV1
|
||||||
|
bool "libcanard UAVCAN v1 Library"
|
||||||
|
default n
|
||||||
|
depends on NET_CAN && ALLOW_MIT_COMPONENTS
|
||||||
|
---help---
|
||||||
|
Enable the libcanard UAVCAN v1 library.
|
||||||
|
|
||||||
|
if CANUTILS_LIBCANARDV1
|
||||||
|
|
||||||
|
config LIBCANARDV1_URL
|
||||||
|
string "libcanard URL"
|
||||||
|
default "https://github.com/UAVCAN/libcanard/archive"
|
||||||
|
---help---
|
||||||
|
libcanard URL.
|
||||||
|
|
||||||
|
config LIBCANARDV1_VERSION
|
||||||
|
string "libcanard Version"
|
||||||
|
default "cde670347425023480a1417fcd603b27c8eb06c1"
|
||||||
|
---help---
|
||||||
|
libcanard version.
|
||||||
|
|
||||||
|
config O1HEAP_URL
|
||||||
|
string "O(1) heap URL"
|
||||||
|
default "https://github.com/pavel-kirienko/o1heap/archive"
|
||||||
|
---help---
|
||||||
|
libcanard URL.
|
||||||
|
|
||||||
|
config O1HEAP_VERSION
|
||||||
|
string "O(1) heap Version"
|
||||||
|
default "b21b069e4b971d3016dd232784faca6f7d9fd724"
|
||||||
|
---help---
|
||||||
|
libcanard version.
|
||||||
|
|
||||||
|
endif
|
23
canutils/libcanardv1/Make.defs
Normal file
23
canutils/libcanardv1/Make.defs
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
############################################################################
|
||||||
|
# apps/canutils/libcanardv1/Make.defs
|
||||||
|
#
|
||||||
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
|
# this work for additional information regarding copyright ownership. The
|
||||||
|
# ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||||
|
# "License"); you may not use this file except in compliance with the
|
||||||
|
# License. You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||||
|
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||||
|
# License for the specific language governing permissions and limitations
|
||||||
|
# under the License.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
ifeq ($(CONFIG_CANUTILS_LIBCANARDV1),y)
|
||||||
|
CONFIGURED_APPS += $(APPDIR)/canutils/libcanardv1
|
||||||
|
endif
|
100
canutils/libcanardv1/Makefile
Normal file
100
canutils/libcanardv1/Makefile
Normal file
@ -0,0 +1,100 @@
|
|||||||
|
############################################################################
|
||||||
|
# apps/canutils/libcanardv1/Makefile
|
||||||
|
#
|
||||||
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
|
# this work for additional information regarding copyright ownership. The
|
||||||
|
# ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||||
|
# "License"); you may not use this file except in compliance with the
|
||||||
|
# License. You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||||
|
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||||
|
# License for the specific language governing permissions and limitations
|
||||||
|
# under the License.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
include $(APPDIR)/Make.defs
|
||||||
|
|
||||||
|
UNPACK = unzip
|
||||||
|
PACKEXT = .zip
|
||||||
|
|
||||||
|
LIBCANARDV1_URL = $(patsubst "%",%,$(strip $(CONFIG_LIBCANARDV1_URL)))
|
||||||
|
LIBCANARDV1_VERSION = $(patsubst "%",%,$(strip $(CONFIG_LIBCANARDV1_VERSION)))
|
||||||
|
LIBCANARDV1_UNPACKNAME = libcanard-$(LIBCANARDV1_VERSION)
|
||||||
|
LIBCANARDV1_PACKNAME = $(LIBCANARDV1_UNPACKNAME)$(PACKEXT)
|
||||||
|
LIBCANARDV1_SRCDIR = $(LIBCANARDV1_UNPACKNAME)
|
||||||
|
LIBCANARDV1_DRVDIR = $(LIBCANARDV1_SRCDIR)$(DELIM)libcanard
|
||||||
|
|
||||||
|
O1HEAP_URL = $(patsubst "%",%,$(strip $(CONFIG_O1HEAP_URL)))
|
||||||
|
O1HEAP_VERSION = $(patsubst "%",%,$(strip $(CONFIG_O1HEAP_VERSION)))
|
||||||
|
O1HEAP_UNPACKNAME = o1heap-$(O1HEAP_VERSION)
|
||||||
|
O1HEAP_PACKNAME = $(O1HEAP_UNPACKNAME)$(PACKEXT)
|
||||||
|
O1HEAP_SRCDIR = $(O1HEAP_UNPACKNAME)
|
||||||
|
O1HEAP_DRVDIR = $(O1HEAP_SRCDIR)$(DELIM)o1heap
|
||||||
|
|
||||||
|
APPS_INCDIR = $(APPDIR)$(DELIM)include$(DELIM)canutils
|
||||||
|
|
||||||
|
CFLAGS += -std=c11 -DCANARD_ASSERT=DEBUGASSERT -DCANARD_DSDL_CONFIG_LITTLE_ENDIAN=1
|
||||||
|
CFLAGS += ${shell $(INCDIR) "$(CC)" $(APPS_INCDIR)}
|
||||||
|
|
||||||
|
CSRCS = $(LIBCANARDV1_DRVDIR)$(DELIM)canard.c $(LIBCANARDV1_DRVDIR)$(DELIM)canard_dsdl.c
|
||||||
|
CSRCS += $(O1HEAP_DRVDIR)$(DELIM)o1heap.c
|
||||||
|
|
||||||
|
|
||||||
|
$(LIBCANARDV1_PACKNAME):
|
||||||
|
@echo "Downloading: $@"
|
||||||
|
$(Q) curl -o $@ -L $(LIBCANARDV1_URL)$(DELIM)$(LIBCANARDV1_VERSION)$(PACKEXT)
|
||||||
|
|
||||||
|
$(LIBCANARDV1_UNPACKNAME): $(LIBCANARDV1_PACKNAME)
|
||||||
|
@echo "Unpacking: $< -> $@"
|
||||||
|
$(call DELDIR, $@)
|
||||||
|
$(Q) $(UNPACK) $<
|
||||||
|
$(Q) touch $@
|
||||||
|
|
||||||
|
$(O1HEAP_PACKNAME):
|
||||||
|
@echo "Downloading: $@"
|
||||||
|
$(Q) curl -o $@ -L $(O1HEAP_URL)$(DELIM)$(O1HEAP_VERSION)$(PACKEXT)
|
||||||
|
|
||||||
|
$(O1HEAP_UNPACKNAME): $(O1HEAP_PACKNAME)
|
||||||
|
@echo "Unpacking: $< -> $@"
|
||||||
|
$(call DELDIR, $@)
|
||||||
|
$(Q) $(UNPACK) $<
|
||||||
|
$(Q) touch $@
|
||||||
|
|
||||||
|
$(LIBCANARDV1_DRVDIR)$(DELIM)canard.h: $(LIBCANARDV1_UNPACKNAME)
|
||||||
|
|
||||||
|
$(LIBCANARDV1_DRVDIR)$(DELIM)canard_dsdl.h: $(LIBCANARDV1_UNPACKNAME)
|
||||||
|
|
||||||
|
$(O1HEAP_DRVDIR)$(DELIM)o1heap.h: $(O1HEAP_UNPACKNAME)
|
||||||
|
|
||||||
|
$(APPS_INCDIR)$(DELIM)canard.h: $(LIBCANARDV1_DRVDIR)$(DELIM)canard.h
|
||||||
|
$(Q) cp $< $@
|
||||||
|
|
||||||
|
$(APPS_INCDIR)$(DELIM)canard_dsdl.h: $(LIBCANARDV1_DRVDIR)$(DELIM)canard_dsdl.h
|
||||||
|
$(Q) cp $< $@
|
||||||
|
|
||||||
|
$(APPS_INCDIR)$(DELIM)o1heap.h: $(O1HEAP_DRVDIR)$(DELIM)o1heap.h
|
||||||
|
$(Q) cp $< $@
|
||||||
|
|
||||||
|
context:: $(APPS_INCDIR)$(DELIM)canard.h $(APPS_INCDIR)$(DELIM)canard_dsdl.h $(APPS_INCDIR)$(DELIM)o1heap.h
|
||||||
|
|
||||||
|
clean::
|
||||||
|
$(foreach OBJ, $(OBJS), $(call DELFILE, $(OBJ)))
|
||||||
|
|
||||||
|
distclean::
|
||||||
|
$(call DELFILE, $(APPS_INCDIR)$(DELIM)canard.h)
|
||||||
|
$(call DELFILE, $(APPS_INCDIR)$(DELIM)canard_dsdl.h)
|
||||||
|
$(call DELDIR, $(LIBCANARDV1_UNPACKNAME))
|
||||||
|
$(call DELFILE, $(LIBCANARDV1_PACKNAME))
|
||||||
|
$(call DELDIR, $(O1HEAP_UNPACKNAME))
|
||||||
|
$(call DELFILE, $(O1HEAP_PACKNAME))
|
||||||
|
$(call DELFILE, $(APPS_INCDIR)$(DELIM)o1heap.h)
|
||||||
|
$(call DELDIR, $(O1HEAP_UNPACKNAME))
|
||||||
|
$(call DELFILE, $(O1HEAP_PACKNAME))
|
||||||
|
|
||||||
|
include $(APPDIR)/Application.mk
|
@ -3,45 +3,45 @@
|
|||||||
# see the file kconfig-language.txt in the NuttX tools repository.
|
# see the file kconfig-language.txt in the NuttX tools repository.
|
||||||
#
|
#
|
||||||
|
|
||||||
config EXAMPLES_LIBCANARD
|
config EXAMPLES_LIBCANARDV0
|
||||||
tristate "libcandard example"
|
tristate "libcandard v0 example"
|
||||||
default n
|
default n
|
||||||
depends on CANUTILS_LIBCANARD && SYSTEM_TIME64
|
depends on CANUTILS_LIBCANARDV0 && SYSTEM_TIME64
|
||||||
---help---
|
---help---
|
||||||
Enable the LIBCANARD example
|
Enable the LIBCANARDV0 example
|
||||||
|
|
||||||
if EXAMPLES_LIBCANARD
|
if EXAMPLES_LIBCANARDV0
|
||||||
|
|
||||||
config EXAMPLES_LIBCANARD_DEVPATH
|
config EXAMPLES_LIBCANARDV0_DEVPATH
|
||||||
string "Device Path"
|
string "Device Path"
|
||||||
default "/dev/can0"
|
default "/dev/can0"
|
||||||
---help---
|
---help---
|
||||||
The device path
|
The device path
|
||||||
|
|
||||||
config EXAMPLES_LIBCANARD_NODE_ID
|
config EXAMPLES_LIBCANARDV0_NODE_ID
|
||||||
int "Node ID"
|
int "Node ID"
|
||||||
default 1
|
default 1
|
||||||
range 1 127
|
range 1 127
|
||||||
---help---
|
---help---
|
||||||
Specifies the node's ID
|
Specifies the node's ID
|
||||||
|
|
||||||
config EXAMPLES_LIBCANARD_APP_NODE_NAME
|
config EXAMPLES_LIBCANARDV0_APP_NODE_NAME
|
||||||
string "Node name"
|
string "Node name"
|
||||||
default "org.uavcan.libcanard.nuttx.demo"
|
default "org.uavcan.libcanard.nuttx.demo"
|
||||||
---help---
|
---help---
|
||||||
app node name
|
app node name
|
||||||
|
|
||||||
config EXAMPLES_LIBCANARD_NODE_MEM_POOL_SIZE
|
config EXAMPLES_LIBCANARDV0_NODE_MEM_POOL_SIZE
|
||||||
int "Node Memory Pool Size"
|
int "Node Memory Pool Size"
|
||||||
default 1024
|
default 1024
|
||||||
---help---
|
---help---
|
||||||
Specifies the node's memory pool size
|
Specifies the node's memory pool size
|
||||||
|
|
||||||
config EXAMPLES_LIBCANARD_DAEMON_PRIORITY
|
config EXAMPLES_LIBCANARDV0_DAEMON_PRIORITY
|
||||||
int "daemon task priority"
|
int "daemon task priority"
|
||||||
default 100
|
default 100
|
||||||
|
|
||||||
config EXAMPLES_LIBCANARD_STACKSIZE
|
config EXAMPLES_LIBCANARDV0_STACKSIZE
|
||||||
int "canard stack size"
|
int "canard stack size"
|
||||||
default DEFAULT_TASK_STACKSIZE
|
default DEFAULT_TASK_STACKSIZE
|
||||||
|
|
@ -1,5 +1,5 @@
|
|||||||
############################################################################
|
############################################################################
|
||||||
# apps/examples/canard/Make.defs
|
# apps/examples/canardv0/Make.defs
|
||||||
#
|
#
|
||||||
# Licensed to the Apache Software Foundation (ASF) under one or more
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
# contributor license agreements. See the NOTICE file distributed with
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
@ -18,6 +18,6 @@
|
|||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
ifneq ($(CONFIG_EXAMPLES_LIBCANARD),)
|
ifneq ($(CONFIG_EXAMPLES_LIBCANARDV0),)
|
||||||
CONFIGURED_APPS += $(APPDIR)/examples/canard
|
CONFIGURED_APPS += $(APPDIR)/examples/canardv0
|
||||||
endif
|
endif
|
@ -1,5 +1,5 @@
|
|||||||
############################################################################
|
############################################################################
|
||||||
# apps/examples/canard/Makefile
|
# apps/examples/canardv0/Makefile
|
||||||
#
|
#
|
||||||
# Licensed to the Apache Software Foundation (ASF) under one or more
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
# contributor license agreements. See the NOTICE file distributed with
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
@ -20,10 +20,10 @@
|
|||||||
|
|
||||||
include $(APPDIR)/Make.defs
|
include $(APPDIR)/Make.defs
|
||||||
|
|
||||||
PROGNAME = canard
|
PROGNAME = canardv0
|
||||||
PRIORITY = SCHED_PRIORITY_DEFAULT
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
STACKSIZE = $(CONFIG_EXAMPLES_LIBCANARD_STACKSIZE)
|
STACKSIZE = $(CONFIG_EXAMPLES_LIBCANARDV0_STACKSIZE)
|
||||||
MODULE = $(CONFIG_EXAMPLES_LIBCANARD)
|
MODULE = $(CONFIG_EXAMPLES_LIBCANARDV0)
|
||||||
|
|
||||||
CFLAGS += ${shell $(INCDIR) "$(CC)" $(APPDIR)/include/canutils}
|
CFLAGS += ${shell $(INCDIR) "$(CC)" $(APPDIR)/include/canutils}
|
||||||
MAINSRC = canard_main.c
|
MAINSRC = canard_main.c
|
@ -1,5 +1,5 @@
|
|||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* apps/examples/canard/canard_main.c
|
* apps/examples/canardv0/canard_main.c
|
||||||
*
|
*
|
||||||
* Copyright (C) 2016 ETH Zuerich. All rights reserved.
|
* Copyright (C) 2016 ETH Zuerich. All rights reserved.
|
||||||
* Author: Matthias Renner <rennerm@ethz.ch>
|
* Author: Matthias Renner <rennerm@ethz.ch>
|
||||||
@ -60,7 +60,7 @@
|
|||||||
|
|
||||||
#define APP_VERSION_MAJOR 1
|
#define APP_VERSION_MAJOR 1
|
||||||
#define APP_VERSION_MINOR 0
|
#define APP_VERSION_MINOR 0
|
||||||
#define APP_NODE_NAME CONFIG_EXAMPLES_LIBCANARD_APP_NODE_NAME
|
#define APP_NODE_NAME CONFIG_EXAMPLES_LIBCANARDV0_APP_NODE_NAME
|
||||||
#define GIT_HASH 0xb28bf6ac
|
#define GIT_HASH 0xb28bf6ac
|
||||||
|
|
||||||
/* Some useful constants defined by the UAVCAN specification.
|
/* Some useful constants defined by the UAVCAN specification.
|
||||||
@ -99,7 +99,7 @@ static CanardInstance canard;
|
|||||||
|
|
||||||
/* Arena for memory allocation, used by the library */
|
/* Arena for memory allocation, used by the library */
|
||||||
|
|
||||||
static uint8_t canard_memory_pool[CONFIG_EXAMPLES_LIBCANARD_NODE_MEM_POOL_SIZE];
|
static uint8_t canard_memory_pool[CONFIG_EXAMPLES_LIBCANARDV0_NODE_MEM_POOL_SIZE];
|
||||||
|
|
||||||
static uint8_t unique_id[UNIQUE_ID_LENGTH_BYTES] =
|
static uint8_t unique_id[UNIQUE_ID_LENGTH_BYTES] =
|
||||||
{ 0x00, 0x00, 0x00, 0x00,
|
{ 0x00, 0x00, 0x00, 0x00,
|
||||||
@ -436,11 +436,11 @@ static int canard_daemon(int argc, char *argv[])
|
|||||||
/* Open the CAN device for reading */
|
/* Open the CAN device for reading */
|
||||||
|
|
||||||
ret = canardNuttXInit(&canardnuttx_instance,
|
ret = canardNuttXInit(&canardnuttx_instance,
|
||||||
CONFIG_EXAMPLES_LIBCANARD_DEVPATH);
|
CONFIG_EXAMPLES_LIBCANARDV0_DEVPATH);
|
||||||
if (ret < 0)
|
if (ret < 0)
|
||||||
{
|
{
|
||||||
printf("canard_daemon: ERROR: open %s failed: %d\n",
|
printf("canard_daemon: ERROR: open %s failed: %d\n",
|
||||||
CONFIG_EXAMPLES_LIBCANARD_DEVPATH, errno);
|
CONFIG_EXAMPLES_LIBCANARDV0_DEVPATH, errno);
|
||||||
errval = 2;
|
errval = 2;
|
||||||
goto errout_with_dev;
|
goto errout_with_dev;
|
||||||
}
|
}
|
||||||
@ -469,9 +469,9 @@ static int canard_daemon(int argc, char *argv[])
|
|||||||
|
|
||||||
canardInit(&canard, canard_memory_pool, sizeof(canard_memory_pool),
|
canardInit(&canard, canard_memory_pool, sizeof(canard_memory_pool),
|
||||||
onTransferReceived, shouldAcceptTransfer, (void *)(12345));
|
onTransferReceived, shouldAcceptTransfer, (void *)(12345));
|
||||||
canardSetLocalNodeID(&canard, CONFIG_EXAMPLES_LIBCANARD_NODE_ID);
|
canardSetLocalNodeID(&canard, CONFIG_EXAMPLES_LIBCANARDV0_NODE_ID);
|
||||||
printf("canard_daemon: canard initialized\n");
|
printf("canard_daemon: canard initialized\n");
|
||||||
printf("start node (ID: %d Name: %s)\n", CONFIG_EXAMPLES_LIBCANARD_NODE_ID,
|
printf("start node (ID: %d Name: %s)\n", CONFIG_EXAMPLES_LIBCANARDV0_NODE_ID,
|
||||||
APP_NODE_NAME);
|
APP_NODE_NAME);
|
||||||
|
|
||||||
g_canard_daemon_started = true;
|
g_canard_daemon_started = true;
|
||||||
@ -517,8 +517,8 @@ int main(int argc, FAR char *argv[])
|
|||||||
return EXIT_SUCCESS;
|
return EXIT_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = task_create("canard_daemon", CONFIG_EXAMPLES_LIBCANARD_DAEMON_PRIORITY,
|
ret = task_create("canard_daemon", CONFIG_EXAMPLES_LIBCANARDV0_DAEMON_PRIORITY,
|
||||||
CONFIG_EXAMPLES_LIBCANARD_STACKSIZE, canard_daemon,
|
CONFIG_EXAMPLES_LIBCANARDV0_STACKSIZE, canard_daemon,
|
||||||
NULL);
|
NULL);
|
||||||
if (ret < 0)
|
if (ret < 0)
|
||||||
{
|
{
|
48
examples/canardv1/Kconfig
Normal file
48
examples/canardv1/Kconfig
Normal file
@ -0,0 +1,48 @@
|
|||||||
|
#
|
||||||
|
# For a description of the syntax of this configuration file,
|
||||||
|
# see the file kconfig-language.txt in the NuttX tools repository.
|
||||||
|
#
|
||||||
|
|
||||||
|
config EXAMPLES_LIBCANARDV1
|
||||||
|
tristate "libcandard v1 example"
|
||||||
|
default n
|
||||||
|
depends on CANUTILS_LIBCANARDV1
|
||||||
|
---help---
|
||||||
|
Enable the LIBCANARDV1 example
|
||||||
|
|
||||||
|
if EXAMPLES_LIBCANARDV1
|
||||||
|
|
||||||
|
config EXAMPLES_LIBCANARDV1_DEV
|
||||||
|
string "Device"
|
||||||
|
default "can0"
|
||||||
|
---help---
|
||||||
|
The device
|
||||||
|
|
||||||
|
config EXAMPLES_LIBCANARDV1_NODE_ID
|
||||||
|
int "Node ID"
|
||||||
|
default 1
|
||||||
|
range 1 127
|
||||||
|
---help---
|
||||||
|
Specifies the node's ID
|
||||||
|
|
||||||
|
config EXAMPLES_LIBCANARDV1_APP_NODE_NAME
|
||||||
|
string "Node name"
|
||||||
|
default "org.uavcan.libcanardv1.nuttx.demo"
|
||||||
|
---help---
|
||||||
|
app node name
|
||||||
|
|
||||||
|
config EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE
|
||||||
|
int "Node Memory Pool Size"
|
||||||
|
default 1024
|
||||||
|
---help---
|
||||||
|
Specifies the node's memory pool size
|
||||||
|
|
||||||
|
config EXAMPLES_LIBCANARDV1_DAEMON_PRIORITY
|
||||||
|
int "daemon task priority"
|
||||||
|
default 100
|
||||||
|
|
||||||
|
config EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE
|
||||||
|
int "canard stack size"
|
||||||
|
default DEFAULT_TASK_STACKSIZE
|
||||||
|
|
||||||
|
endif
|
23
examples/canardv1/Make.defs
Normal file
23
examples/canardv1/Make.defs
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
############################################################################
|
||||||
|
# apps/examples/canardv1/Make.defs
|
||||||
|
#
|
||||||
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
|
# this work for additional information regarding copyright ownership. The
|
||||||
|
# ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||||
|
# "License"); you may not use this file except in compliance with the
|
||||||
|
# License. You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http:#www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||||
|
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||||
|
# License for the specific language governing permissions and limitations
|
||||||
|
# under the License.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
ifneq ($(CONFIG_EXAMPLES_LIBCANARDV1),)
|
||||||
|
CONFIGURED_APPS += $(APPDIR)/examples/canardv1
|
||||||
|
endif
|
32
examples/canardv1/Makefile
Normal file
32
examples/canardv1/Makefile
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
############################################################################
|
||||||
|
# apps/examples/canardv1/Makefile
|
||||||
|
#
|
||||||
|
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
|
# contributor license agreements. See the NOTICE file distributed with
|
||||||
|
# this work for additional information regarding copyright ownership. The
|
||||||
|
# ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||||
|
# "License"); you may not use this file except in compliance with the
|
||||||
|
# License. You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http:#www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||||
|
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||||
|
# License for the specific language governing permissions and limitations
|
||||||
|
# under the License.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
include $(APPDIR)/Make.defs
|
||||||
|
|
||||||
|
PROGNAME = canardv1
|
||||||
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
|
STACKSIZE = $(CONFIG_EXAMPLES_LIBCANARDV1_DAEMON_STACK_SIZE)
|
||||||
|
MODULE = $(CONFIG_EXAMPLES_LIBCANARDV1)
|
||||||
|
|
||||||
|
CFLAGS += -std=c11 ${shell $(INCDIR) "$(CC)" $(APPDIR)/include/canutils $(APPDIR)/include/o1heap}
|
||||||
|
CSRCS = socketcan.c
|
||||||
|
MAINSRC = canard_main.c
|
||||||
|
|
||||||
|
include $(APPDIR)/Application.mk
|
412
examples/canardv1/canard_main.c
Normal file
412
examples/canardv1/canard_main.c
Normal file
@ -0,0 +1,412 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
* apps/examples/canardv1/canard_main.c
|
||||||
|
*
|
||||||
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
|
* contributor license agreements. See the NOTICE file distributed with
|
||||||
|
* this work for additional information regarding copyright ownership. The
|
||||||
|
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||||
|
* "License"); you may not use this file except in compliance with the
|
||||||
|
* License. You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||||
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||||
|
* License for the specific language governing permissions and limitations
|
||||||
|
* under the License.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Included Files
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include <canard.h>
|
||||||
|
#include <canard_dsdl.h>
|
||||||
|
#include <o1heap.h>
|
||||||
|
|
||||||
|
#include <sched.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"
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Pre-processor Definitions
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/* Application constants */
|
||||||
|
|
||||||
|
#define APP_VERSION_MAJOR 1
|
||||||
|
#define APP_VERSION_MINOR 0
|
||||||
|
#define APP_NODE_NAME CONFIG_EXAMPLES_LIBCANARDV1_APP_NODE_NAME
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Private Data
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/* Arena for memory allocation, used by the library */
|
||||||
|
|
||||||
|
#define O1_HEAP_SIZE CONFIG_EXAMPLES_LIBCANARDV1_NODE_MEM_POOL_SIZE
|
||||||
|
|
||||||
|
/* Temporary development UAVCAN topic service ID to publish/subscribe from */
|
||||||
|
#define PORT_ID 4421
|
||||||
|
#define TOPIC_SIZE 512
|
||||||
|
|
||||||
|
O1HeapInstance *my_allocator;
|
||||||
|
static uint8_t uavcan_heap[O1_HEAP_SIZE]
|
||||||
|
__attribute__((aligned(O1HEAP_ALIGNMENT)));
|
||||||
|
|
||||||
|
/* Node status variables */
|
||||||
|
|
||||||
|
static bool g_canard_daemon_started;
|
||||||
|
|
||||||
|
static uint8_t my_message_transfer_id;
|
||||||
|
|
||||||
|
struct pollfd fd;
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* 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)
|
||||||
|
{
|
||||||
|
struct timespec ts;
|
||||||
|
|
||||||
|
memset(&ts, 0, sizeof(ts));
|
||||||
|
|
||||||
|
if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0)
|
||||||
|
{
|
||||||
|
abort();
|
||||||
|
}
|
||||||
|
|
||||||
|
return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Name: process1hztasks
|
||||||
|
*
|
||||||
|
* Description:
|
||||||
|
* This function is called at 1 Hz rate from the main loop.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
void process1hztasks(CanardInstance *ins, uint64_t timestamp_usec)
|
||||||
|
{
|
||||||
|
CanardMicrosecond transmission_deadline =
|
||||||
|
getmonotonictimestampusec() + 1000 * 10;
|
||||||
|
|
||||||
|
const CanardTransfer transfer =
|
||||||
|
{
|
||||||
|
.timestamp_usec = transmission_deadline, /* Zero if transmission deadline is not limited. */
|
||||||
|
.priority = CanardPriorityNominal,
|
||||||
|
.transfer_kind = CanardTransferKindMessage,
|
||||||
|
.port_id = 1234, /* This is the subject-ID. */
|
||||||
|
.remote_node_id = CANARD_NODE_ID_UNSET, /* Messages cannot be unicast, so use UNSET. */
|
||||||
|
.transfer_id = my_message_transfer_id,
|
||||||
|
.payload_size = 47,
|
||||||
|
.payload = "\x2D\x00"
|
||||||
|
"Sancho, it strikes me thou art in great fear.",
|
||||||
|
};
|
||||||
|
|
||||||
|
++my_message_transfer_id; /* The transfer-ID shall be incremented after every transmission on this subject. */
|
||||||
|
int32_t 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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
fprintf(stderr, "Transmit error %ld\n", result);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* 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, Look at the top of the TX queue. */
|
||||||
|
|
||||||
|
for (const CanardFrame *txf = NULL; (txf = canardTxPeek(ins)) != NULL; )
|
||||||
|
{
|
||||||
|
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. */
|
||||||
|
{
|
||||||
|
/* If the driver is busy, break and retry later. */
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
canardTxPop(ins); /* Remove the frame from the queue after it's transmitted. */
|
||||||
|
ins->memory_free(ins, (CanardFrame *)txf);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 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 %ld\n", result);
|
||||||
|
}
|
||||||
|
else if (result == 1)
|
||||||
|
{
|
||||||
|
/* A transfer has been received, process it */
|
||||||
|
|
||||||
|
printf("Receive UAVCAN port id%d TODO process me\n",
|
||||||
|
receive.port_id);
|
||||||
|
|
||||||
|
ins->memory_free(ins, (void *)receive.payload);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
/* 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;
|
||||||
|
int pub = 1;
|
||||||
|
|
||||||
|
if (argc > 2)
|
||||||
|
{
|
||||||
|
for (int args = 2; args < argc; args++)
|
||||||
|
{
|
||||||
|
if (!strcmp(argv[args], "canfd"))
|
||||||
|
{
|
||||||
|
can_fd = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[args], "pub"))
|
||||||
|
{
|
||||||
|
pub = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[args], "sub"))
|
||||||
|
{
|
||||||
|
pub = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
my_allocator = o1heapInit(&uavcan_heap, O1_HEAP_SIZE);
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
ins.node_id = (pub ? CONFIG_EXAMPLES_LIBCANARDV1_NODE_ID :
|
||||||
|
CONFIG_EXAMPLES_LIBCANARDV1_NODE_ID + 1);
|
||||||
|
|
||||||
|
/* 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("canard_daemon: canard initialized\n");
|
||||||
|
printf("start node (ID: %d Name: %s MTU: %d PUB: %d TOPIC_SIZE: %d)\n",
|
||||||
|
ins.node_id, APP_NODE_NAME, ins.mtu_bytes, pub, TOPIC_SIZE);
|
||||||
|
|
||||||
|
CanardRxSubscription heartbeat_subscription;
|
||||||
|
(void) canardRxSubscribe(&ins, /* Subscribe to messages uavcan.node.Heartbeat. */
|
||||||
|
CanardTransferKindMessage,
|
||||||
|
32085, /* The fixed Subject-ID of the Heartbeat message type (see DSDL definition). */
|
||||||
|
7, /* The maximum payload size (max DSDL object size) from the DSDL definition. */
|
||||||
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
|
&heartbeat_subscription);
|
||||||
|
|
||||||
|
CanardRxSubscription my_subscription;
|
||||||
|
(void) canardRxSubscribe(&ins,
|
||||||
|
CanardTransferKindMessage,
|
||||||
|
PORT_ID, /* The Service-ID to subscribe to. */
|
||||||
|
TOPIC_SIZE, /* The maximum payload size (max DSDL object size). */
|
||||||
|
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
|
||||||
|
&my_subscription);
|
||||||
|
|
||||||
|
g_canard_daemon_started = true;
|
||||||
|
uint64_t next_1hz_service_at = getmonotonictimestampusec();
|
||||||
|
|
||||||
|
for (; ; )
|
||||||
|
{
|
||||||
|
processtxrxonce(&ins, &sock_ins, 10);
|
||||||
|
|
||||||
|
const uint64_t ts = getmonotonictimestampusec();
|
||||||
|
|
||||||
|
if (ts >= next_1hz_service_at)
|
||||||
|
{
|
||||||
|
next_1hz_service_at += 1000000;
|
||||||
|
process1hztasks(&ins, ts);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
errout_with_dev:
|
||||||
|
|
||||||
|
g_canard_daemon_started = false;
|
||||||
|
printf("canard_daemon: Terminating!\n");
|
||||||
|
fflush(stdout);
|
||||||
|
return errval;
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Name: canard_main
|
||||||
|
*
|
||||||
|
* Description:
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
int canardv1_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;
|
||||||
|
}
|
227
examples/canardv1/socketcan.c
Normal file
227
examples/canardv1/socketcan.c
Normal file
@ -0,0 +1,227 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
* apps/examples/canardv1/socketcan.c
|
||||||
|
*
|
||||||
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
|
* contributor license agreements. See the NOTICE file distributed with
|
||||||
|
* this work for additional information regarding copyright ownership. The
|
||||||
|
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||||
|
* "License"); you may not use this file except in compliance with the
|
||||||
|
* License. You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||||
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||||
|
* License for the specific language governing permissions and limitations
|
||||||
|
* under the License.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Included Files
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "socketcan.h"
|
||||||
|
|
||||||
|
#include <net/if.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Public Functions
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 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;
|
||||||
|
}
|
114
examples/canardv1/socketcan.h
Normal file
114
examples/canardv1/socketcan.h
Normal file
@ -0,0 +1,114 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
* apps/examples/canardv1/socketcan.h
|
||||||
|
*
|
||||||
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||||
|
* contributor license agreements. See the NOTICE file distributed with
|
||||||
|
* this work for additional information regarding copyright ownership. The
|
||||||
|
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||||
|
* "License"); you may not use this file except in compliance with the
|
||||||
|
* License. You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||||
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||||
|
* License for the specific language governing permissions and limitations
|
||||||
|
* under the License.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#ifndef SOCKETCAN_H_INCLUDED
|
||||||
|
#define SOCKETCAN_H_INCLUDED
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Included Files
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#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
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Public Types
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
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)];
|
||||||
|
};
|
||||||
|
|
||||||
|
/****************************************************************************
|
||||||
|
* Public Function Prototypes
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/* Creates a SocketCAN socket for corresponding iface can_iface_name
|
||||||
|
* Also sets up the message structures required for transmit and receive
|
||||||
|
* 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
|
2
include/canutils/.gitignore
vendored
2
include/canutils/.gitignore
vendored
@ -1,2 +1,4 @@
|
|||||||
/canard.h
|
/canard.h
|
||||||
/canard_nuttx.h
|
/canard_nuttx.h
|
||||||
|
/canard_dsdl.h
|
||||||
|
/o1heap.h
|
||||||
|
Loading…
x
Reference in New Issue
Block a user