This is an automated email from the ASF dual-hosted git repository. acassis pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
commit c785d40af7b354a5ee312ad41e174e07875866da Author: raiden00pl <[email protected]> AuthorDate: Sun Jul 5 09:52:51 2026 +0200 examples: add Lely CANopen slave example add Lely CANopen slave example Signed-off-by: raiden00pl <[email protected]> --- examples/lely_slave/CMakeLists.txt | 55 ++++ examples/lely_slave/Kconfig | 49 +++ examples/lely_slave/Make.defs | 25 ++ examples/lely_slave/Makefile | 47 +++ examples/lely_slave/candev.h | 44 +++ examples/lely_slave/candev_char.c | 173 +++++++++++ examples/lely_slave/candev_sock.c | 197 ++++++++++++ examples/lely_slave/sdev.h | 41 +++ examples/lely_slave/sdev_slave.c | 594 +++++++++++++++++++++++++++++++++++++ examples/lely_slave/slave_main.c | 242 +++++++++++++++ 10 files changed, 1467 insertions(+) diff --git a/examples/lely_slave/CMakeLists.txt b/examples/lely_slave/CMakeLists.txt new file mode 100644 index 000000000..4ffdb62ea --- /dev/null +++ b/examples/lely_slave/CMakeLists.txt @@ -0,0 +1,55 @@ +# ############################################################################## +# apps/examples/lely_slave/CMakeLists.txt +# +# SPDX-License-Identifier: Apache-2.0 +# +# 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. +# +# ############################################################################## + +if(CONFIG_EXAMPLES_LELYSLAVE) + + set(LELY_INCDIR ${NUTTX_APPS_DIR}/canutils/lely-canopen/lely-core/include) + + # The main source must be first so it receives the main=coslave_main alias + + set(SRCS slave_main.c sdev_slave.c) + + if(CONFIG_EXAMPLES_LELYSLAVE_CHARDEV) + list(APPEND SRCS candev_char.c) + endif() + + if(CONFIG_EXAMPLES_LELYSLAVE_SOCKET) + list(APPEND SRCS candev_sock.c) + endif() + + nuttx_add_application( + NAME + coslave + PRIORITY + ${CONFIG_EXAMPLES_LELYSLAVE_PRIORITY} + STACKSIZE + ${CONFIG_EXAMPLES_LELYSLAVE_STACKSIZE} + MODULE + ${CONFIG_EXAMPLES_LELYSLAVE} + COMPILE_FLAGS + -Wno-undef + INCLUDE_DIRECTORIES + ${LELY_INCDIR} + SRCS + ${SRCS}) + +endif() diff --git a/examples/lely_slave/Kconfig b/examples/lely_slave/Kconfig new file mode 100644 index 000000000..de8f9d30a --- /dev/null +++ b/examples/lely_slave/Kconfig @@ -0,0 +1,49 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config EXAMPLES_LELYSLAVE + tristate "Lely CANopen slave example" + default n + depends on CANUTILS_LELYCANOPEN && CANUTILS_LELYCANOPEN_SDEV + depends on CANUTILS_LELYCANOPEN_TIME && CANUTILS_LELYCANOPEN_OBJUPLOAD + ---help--- + Enable the Lely CANopen slave (coslave) example application. + +if EXAMPLES_LELYSLAVE + +choice + prompt "CAN backend" + default EXAMPLES_LELYSLAVE_CHARDEV + +config EXAMPLES_LELYSLAVE_CHARDEV + bool "CAN character driver" + depends on CAN + +config EXAMPLES_LELYSLAVE_SOCKET + bool "SocketCAN" + depends on NET_CAN + select NETUTILS_NETLIB + +endchoice + +config EXAMPLES_LELYSLAVE_DEVPATH + string "CAN character device path" + default "/dev/can0" + depends on EXAMPLES_LELYSLAVE_CHARDEV + +config EXAMPLES_LELYSLAVE_INTF + string "SocketCAN interface" + default "can0" + depends on EXAMPLES_LELYSLAVE_SOCKET + +config EXAMPLES_LELYSLAVE_PRIORITY + int "Task priority" + default 100 + +config EXAMPLES_LELYSLAVE_STACKSIZE + int "Stack size" + default DEFAULT_TASK_STACKSIZE + +endif diff --git a/examples/lely_slave/Make.defs b/examples/lely_slave/Make.defs new file mode 100644 index 000000000..20382554f --- /dev/null +++ b/examples/lely_slave/Make.defs @@ -0,0 +1,25 @@ +############################################################################ +# apps/examples/lely_slave/Make.defs +# +# SPDX-License-Identifier: Apache-2.0 +# +# 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_LELYSLAVE),) +CONFIGURED_APPS += $(APPDIR)/examples/lely_slave +endif diff --git a/examples/lely_slave/Makefile b/examples/lely_slave/Makefile new file mode 100644 index 000000000..dcc135012 --- /dev/null +++ b/examples/lely_slave/Makefile @@ -0,0 +1,47 @@ +############################################################################ +# apps/examples/lely_slave/Makefile +# +# SPDX-License-Identifier: Apache-2.0 +# +# 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 = coslave +PRIORITY = $(CONFIG_EXAMPLES_LELYSLAVE_PRIORITY) +STACKSIZE = $(CONFIG_EXAMPLES_LELYSLAVE_STACKSIZE) +MODULE = $(CONFIG_EXAMPLES_LELYSLAVE) + +CFLAGS += -DHAVE_CONFIG_H=1 + +# Lely headers use bare "#if __cplusplus"; silence -Wundef like the library + +CFLAGS += -Wno-undef + +CSRCS = sdev_slave.c + +ifeq ($(CONFIG_EXAMPLES_LELYSLAVE_CHARDEV),y) +CSRCS += candev_char.c +endif +ifeq ($(CONFIG_EXAMPLES_LELYSLAVE_SOCKET),y) +CSRCS += candev_sock.c +endif + +MAINSRC = slave_main.c + +include $(APPDIR)/Application.mk diff --git a/examples/lely_slave/candev.h b/examples/lely_slave/candev.h new file mode 100644 index 000000000..e044f8e89 --- /dev/null +++ b/examples/lely_slave/candev.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * apps/examples/lely_slave/candev.h + * + * SPDX-License-Identifier: Apache-2.0 + * + * 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 __APPS_EXAMPLES_LELY_SLAVE_CANDEV_H +#define __APPS_EXAMPLES_LELY_SLAVE_CANDEV_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <canutils/lely/config.h> + +#include <lely/can/msg.h> + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +int coslave_candev_init(void); +int coslave_candev_send(FAR const struct can_msg *msg); +int coslave_candev_recv(FAR struct can_msg *msg); + +#endif /* __APPS_EXAMPLES_LELY_SLAVE_CANDEV_H */ diff --git a/examples/lely_slave/candev_char.c b/examples/lely_slave/candev_char.c new file mode 100644 index 000000000..71c2aed11 --- /dev/null +++ b/examples/lely_slave/candev_char.c @@ -0,0 +1,173 @@ +/**************************************************************************** + * apps/examples/lely_slave/candev_char.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * 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 <nuttx/config.h> + +#include <stdio.h> +#include <fcntl.h> +#include <errno.h> +#include <unistd.h> + +#include <sys/types.h> +#include <sys/ioctl.h> + +#include <nuttx/can/can.h> + +#include "candev.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static int g_candev_fd; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: coslave_candev_init + ****************************************************************************/ + +int coslave_candev_init(void) +{ + struct canioc_bittiming_s bt; + int ret; + + /* Non-blocking so the main loop keeps servicing the CANopen timers */ + + g_candev_fd = open(CONFIG_EXAMPLES_LELYSLAVE_DEVPATH, + O_RDWR | O_NONBLOCK); + if (g_candev_fd < 0) + { + ret = -errno; + printf("ERROR: open %s failed %d\n", + CONFIG_EXAMPLES_LELYSLAVE_DEVPATH, errno); + return ret; + } + + ret = ioctl(g_candev_fd, CANIOC_GET_BITTIMING, &bt); + if (ret < 0) + { + printf("Bit timing not available: %d\n", errno); + } + else + { + printf("Bit timing:\n"); + printf(" Baud: %lu\n", (unsigned long)bt.bt_baud); + printf(" TSEG1: %u\n", bt.bt_tseg1); + printf(" TSEG2: %u\n", bt.bt_tseg2); + printf(" SJW: %u\n", bt.bt_sjw); + } + + return OK; +} + +/**************************************************************************** + * Name: coslave_candev_send + ****************************************************************************/ + +int coslave_candev_send(FAR const struct can_msg *msg) +{ + struct can_msg_s txmsg; + ssize_t nwritten; + int i; + + txmsg.cm_hdr.ch_id = msg->id; + txmsg.cm_hdr.ch_rtr = (msg->flags & CAN_FLAG_RTR) != 0; + txmsg.cm_hdr.ch_dlc = msg->len; +#ifdef CONFIG_CAN_EXTID + txmsg.cm_hdr.ch_extid = (msg->flags & CAN_FLAG_IDE) != 0; +#endif +#ifdef CONFIG_CAN_ERRORS + txmsg.cm_hdr.ch_error = 0; +#endif + txmsg.cm_hdr.ch_tcf = 0; + + for (i = 0; i < msg->len; i++) + { + txmsg.cm_data[i] = msg->data[i]; + } + + nwritten = write(g_candev_fd, &txmsg, CAN_MSGLEN(txmsg.cm_hdr.ch_dlc)); + if (nwritten != CAN_MSGLEN(txmsg.cm_hdr.ch_dlc)) + { + return -1; + } + + return 0; +} + +/**************************************************************************** + * Name: coslave_candev_recv + ****************************************************************************/ + +int coslave_candev_recv(FAR struct can_msg *msg) +{ + struct can_msg_s rxmsg; + ssize_t nread; + int i; + + nread = read(g_candev_fd, &rxmsg, sizeof(struct can_msg_s)); + if (nread < 0) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + { + return 0; + } + + return -1; + } + + if (nread < CAN_MSGLEN(0)) + { + return 0; + } + + msg->id = rxmsg.cm_hdr.ch_id; + msg->flags = 0; + +#ifdef CONFIG_CAN_EXTID + if (rxmsg.cm_hdr.ch_extid) + { + msg->flags |= CAN_FLAG_IDE; + } +#endif + + if (rxmsg.cm_hdr.ch_rtr) + { + msg->flags |= CAN_FLAG_RTR; + } + + msg->len = rxmsg.cm_hdr.ch_dlc; + + for (i = 0; i < rxmsg.cm_hdr.ch_dlc; i++) + { + msg->data[i] = rxmsg.cm_data[i]; + } + + return 1; +} diff --git a/examples/lely_slave/candev_sock.c b/examples/lely_slave/candev_sock.c new file mode 100644 index 000000000..d52db0b97 --- /dev/null +++ b/examples/lely_slave/candev_sock.c @@ -0,0 +1,197 @@ +/**************************************************************************** + * apps/examples/lely_slave/candev_sock.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * 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 <nuttx/config.h> + +#include <stdio.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <unistd.h> + +#include <net/if.h> +#include <sys/socket.h> + +#include <nuttx/can.h> + +#include "netutils/netlib.h" +#include "candev.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static int g_candev_sock; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: coslave_candev_init + ****************************************************************************/ + +int coslave_candev_init(void) +{ + struct sockaddr_can addr; + struct ifreq ifr; + int on = 1; + int ret; + + g_candev_sock = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (g_candev_sock < 0) + { + ret = -errno; + printf("ERROR: socket failed %d\n", errno); + return ret; + } + + strlcpy(ifr.ifr_name, CONFIG_EXAMPLES_LELYSLAVE_INTF, IFNAMSIZ); + + /* SocketCAN silently drops traffic while the interface is down, so bring + * it up here. This lets the example run without a separate "ifup" step. + */ + + if (netlib_ifup(ifr.ifr_name) < 0) + { + ret = -errno; + printf("ERROR: netlib_ifup %s failed %d\n", ifr.ifr_name, errno); + goto errout; + } + + ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); + if (ifr.ifr_ifindex == 0) + { + ret = -errno; + printf("ERROR: if_nametoindex %s failed %d\n", ifr.ifr_name, errno); + goto errout; + } + + setsockopt(g_candev_sock, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, + &on, sizeof(on)); + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(g_candev_sock, (FAR struct sockaddr *)&addr, sizeof(addr)) < 0) + { + ret = -errno; + printf("ERROR: bind failed %d\n", errno); + goto errout; + } + + ret = fcntl(g_candev_sock, F_SETFL, O_NONBLOCK); + if (ret < 0) + { + ret = -errno; + goto errout; + } + + return OK; + +errout: + close(g_candev_sock); + g_candev_sock = -1; + return ret; +} + +/**************************************************************************** + * Name: coslave_candev_send + ****************************************************************************/ + +int coslave_candev_send(FAR const struct can_msg *msg) +{ + struct can_frame frame; + ssize_t nwritten; + + memset(&frame, 0, sizeof(frame)); + + frame.can_id = msg->id; + if (msg->flags & CAN_FLAG_IDE) + { + frame.can_id |= CAN_EFF_FLAG; + } + + if (msg->flags & CAN_FLAG_RTR) + { + frame.can_id |= CAN_RTR_FLAG; + } + + frame.can_dlc = msg->len; + memcpy(frame.data, msg->data, msg->len); + + nwritten = write(g_candev_sock, &frame, sizeof(frame)); + if (nwritten != sizeof(frame)) + { + return -1; + } + + return 0; +} + +/**************************************************************************** + * Name: coslave_candev_recv + ****************************************************************************/ + +int coslave_candev_recv(FAR struct can_msg *msg) +{ + struct can_frame frame; + ssize_t nread; + + nread = read(g_candev_sock, &frame, sizeof(frame)); + if (nread < 0) + { + if (errno == EAGAIN || errno == EWOULDBLOCK) + { + return 0; + } + + return -1; + } + + if (nread < sizeof(frame)) + { + return 0; + } + + msg->id = frame.can_id & CAN_EFF_MASK; + msg->flags = 0; + if (frame.can_id & CAN_EFF_FLAG) + { + msg->flags |= CAN_FLAG_IDE; + } + + if (frame.can_id & CAN_RTR_FLAG) + { + msg->flags |= CAN_FLAG_RTR; + } + + msg->len = frame.can_dlc; + memcpy(msg->data, frame.data, frame.can_dlc); + + return 1; +} diff --git a/examples/lely_slave/sdev.h b/examples/lely_slave/sdev.h new file mode 100644 index 000000000..ca13a532d --- /dev/null +++ b/examples/lely_slave/sdev.h @@ -0,0 +1,41 @@ +/**************************************************************************** + * apps/examples/lely_slave/sdev.h + * + * SPDX-License-Identifier: Apache-2.0 + * + * 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 __APPS_EXAMPLES_LELY_SDEV_H +#define __APPS_EXAMPLES_LELY_SDEV_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <canutils/lely/config.h> +#include <lely/co/sdev.h> + +/**************************************************************************** + * Public data + ****************************************************************************/ + +extern const struct co_sdev g_sdev_slave; + +#endif /* __APPS_EXAMPLES_LELY_SDEV_H */ diff --git a/examples/lely_slave/sdev_slave.c b/examples/lely_slave/sdev_slave.c new file mode 100644 index 000000000..8f71558f5 --- /dev/null +++ b/examples/lely_slave/sdev_slave.c @@ -0,0 +1,594 @@ +/**************************************************************************** + * apps/examples/lely_slave/sdev_slave.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * 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 <nuttx/config.h> + +#include "sdev.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define CO_SDEV_STRING(s) s + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* Object 0x1000 sub-objects */ + +static const struct co_ssub g_1000_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Device type"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = CO_UNSIGNED32_MIN, + .access = CO_ACCESS_RO, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1001 sub-objects */ + +static const struct co_ssub g_1001_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Error register"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED8, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u8 = CO_UNSIGNED8_MIN, + .max.u8 = CO_UNSIGNED8_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u8 = CO_UNSIGNED8_MIN, +#endif + .val.u8 = CO_UNSIGNED8_MIN, + .access = CO_ACCESS_RO, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1005 sub-objects */ + +static const struct co_ssub g_1005_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("COB-ID SYNC message"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = 0x00000080lu, +#endif + .val.u32 = 0x00000080lu, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1012 sub-objects */ + +static const struct co_ssub g_1012_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("COB-ID time stamp object"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = 0x80000100lu, +#endif + .val.u32 = 0x80000100lu, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1017 sub-objects */ + +static const struct co_ssub g_1017_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Producer heartbeat time"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED16, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u16 = CO_UNSIGNED16_MIN, + .max.u16 = CO_UNSIGNED16_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u16 = 0x0032, +#endif + .val.u16 = 0x0032, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1018 sub-objects */ + +static const struct co_ssub g_1018_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Highest sub-index supported"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED8, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u8 = CO_UNSIGNED8_MIN, + .max.u8 = CO_UNSIGNED8_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u8 = 0x04, +#endif + .val.u8 = 0x04, + .access = CO_ACCESS_CONST, + .pdo_mapping = 0, + .flags = 0 + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Vendor-ID"), +#endif + .subidx = 0x01, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = 0x00000360lu, +#endif + .val.u32 = 0x00000360lu, + .access = CO_ACCESS_RO, + .pdo_mapping = 0, + .flags = 0 + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Product code"), +#endif + .subidx = 0x02, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = CO_UNSIGNED32_MIN, + .access = CO_ACCESS_RO, + .pdo_mapping = 0, + .flags = 0 + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Revision number"), +#endif + .subidx = 0x03, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = CO_UNSIGNED32_MIN, + .access = CO_ACCESS_RO, + .pdo_mapping = 0, + .flags = 0 + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Serial number"), +#endif + .subidx = 0x04, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = CO_UNSIGNED32_MIN, + .access = CO_ACCESS_RO, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1800 sub-objects */ + +static const struct co_ssub g_1800_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Highest sub-index supported"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED8, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u8 = CO_UNSIGNED8_MIN, + .max.u8 = CO_UNSIGNED8_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u8 = 0x02, +#endif + .val.u8 = 0x02, + .access = CO_ACCESS_CONST, + .pdo_mapping = 0, + .flags = 0 + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("COB-ID"), +#endif + .subidx = 0x01, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = 0x00000182lu, +#endif + .val.u32 = 0x00000182lu, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Transmission type"), +#endif + .subidx = 0x02, + .type = CO_DEFTYPE_UNSIGNED8, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u8 = CO_UNSIGNED8_MIN, + .max.u8 = CO_UNSIGNED8_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u8 = 0x01, +#endif + .val.u8 = 0x01, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1a00 sub-objects */ + +static const struct co_ssub g_1a00_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Number of mapped objects"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED8, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u8 = CO_UNSIGNED8_MIN, + .max.u8 = CO_UNSIGNED8_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u8 = 0x01, +#endif + .val.u8 = 0x01, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Mapping 1"), +#endif + .subidx = 0x01, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = 0x21000020lu, +#endif + .val.u32 = 0x21000020lu, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x1f80 sub-objects */ + +static const struct co_ssub g_1f80_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("NMT startup"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = 0x00000004lu, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + | CO_OBJ_FLAGS_PARAMETER_VALUE + } +}; + +/* Object 0x2000 sub-objects */ + +static const struct co_ssub g_2000_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Object with custom SDO download callback"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = CO_UNSIGNED32_MIN, + .access = CO_ACCESS_RW, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x2001 sub-objects */ + +static const struct co_ssub g_2001_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Object with custom SDO upload callback"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = CO_UNSIGNED32_MIN, + .access = CO_ACCESS_RO, + .pdo_mapping = 0, + .flags = 0 + } +}; + +/* Object 0x2100 sub-objects */ + +static const struct co_ssub g_2100_sub[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Counter"), +#endif + .subidx = 0x00, + .type = CO_DEFTYPE_UNSIGNED32, +#if !LELY_NO_CO_OBJ_LIMITS + .min.u32 = CO_UNSIGNED32_MIN, + .max.u32 = CO_UNSIGNED32_MAX, +#endif +#if !LELY_NO_CO_OBJ_DEFAULT + .def.u32 = CO_UNSIGNED32_MIN, +#endif + .val.u32 = CO_UNSIGNED32_MIN, + .access = CO_ACCESS_RW, + .pdo_mapping = 1, + .flags = 0 + } +}; + +/* Object dictionary */ + +static const struct co_sobj g_g_sdev_slave_objs[] = +{ + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Device type"), +#endif + .idx = 0x1000, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_1000_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Error register"), +#endif + .idx = 0x1001, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_1001_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("COB-ID SYNC message"), +#endif + .idx = 0x1005, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_1005_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("COB-ID time stamp object"), +#endif + .idx = 0x1012, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_1012_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Producer heartbeat time"), +#endif + .idx = 0x1017, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_1017_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Identity object"), +#endif + .idx = 0x1018, + .code = CO_OBJECT_RECORD, + .nsub = 5, + .subs = g_1018_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("TPDO communication parameter"), +#endif + .idx = 0x1800, + .code = CO_OBJECT_RECORD, + .nsub = 3, + .subs = g_1800_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("TPDO mapping parameter"), +#endif + .idx = 0x1a00, + .code = CO_OBJECT_RECORD, + .nsub = 2, + .subs = g_1a00_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("NMT startup"), +#endif + .idx = 0x1f80, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_1f80_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Object with custom SDO download callback"), +#endif + .idx = 0x2000, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_2000_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Object with custom SDO upload callback"), +#endif + .idx = 0x2001, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_2001_sub + }, + { +#if !LELY_NO_CO_OBJ_NAME + .name = CO_SDEV_STRING("Counter"), +#endif + .idx = 0x2100, + .code = CO_OBJECT_VAR, + .nsub = 1, + .subs = g_2100_sub + } +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +const struct co_sdev g_sdev_slave = +{ + .id = 0x02, + .name = NULL, + .vendor_name = CO_SDEV_STRING("Apache NuttX RTOS"), + .vendor_id = 0x00000000, + .product_name = NULL, + .product_code = 0x00000000, + .revision = 0x00000000, + .order_code = NULL, + .baud = CO_BAUD_125, + .rate = 0, + .lss = 0, + .dummy = 0x000000fe, + .nobj = 12, + .objs = g_g_sdev_slave_objs +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ diff --git a/examples/lely_slave/slave_main.c b/examples/lely_slave/slave_main.c new file mode 100644 index 000000000..39cfa8af7 --- /dev/null +++ b/examples/lely_slave/slave_main.c @@ -0,0 +1,242 @@ +/**************************************************************************** + * apps/examples/lely_slave/slave_main.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * 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 <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> +#include <unistd.h> + +#include <canutils/lely/config.h> + +#include <lely/co/dev.h> +#include <lely/co/nmt.h> +#include <lely/co/sdev.h> +#include <lely/co/sdo.h> +#include <lely/co/time.h> + +#include "sdev.h" +#include "candev.h" + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct co_net_s +{ + can_net_t *net; + co_dev_t *dev; + co_nmt_t *nmt; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct co_net_s g_net; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: elapsed_ms + ****************************************************************************/ + +static long elapsed_ms(const struct timespec *a, const struct timespec *b) +{ + return (a->tv_sec - b->tv_sec) * 1000 + + (a->tv_nsec - b->tv_nsec) / 1000000; +} + +/**************************************************************************** + * Name: on_can_send + ****************************************************************************/ + +static int on_can_send(const struct can_msg *msg, void *data) +{ + UNUSED(data); + + return coslave_candev_send(msg); +} + +/**************************************************************************** + * Name: on_time + ****************************************************************************/ + +static void on_time(co_time_t *time, const struct timespec *tp, void *data) +{ + UNUSED(time); + UNUSED(data); + + /* Update the wall clock */ + + clock_settime(CLOCK_REALTIME, tp); +} + +/**************************************************************************** + * Name: on_nmt_cs + ****************************************************************************/ + +static void on_nmt_cs(co_nmt_t *nmt, co_unsigned8_t cs, void *data) +{ + UNUSED(data); + + switch (cs) + { + case CO_NMT_CS_START: + case CO_NMT_CS_ENTER_PREOP: + { + co_time_set_ind(co_nmt_get_time(nmt), &on_time, NULL); + break; + } + + case CO_NMT_CS_RESET_NODE: + { + exit(0); + break; + } + + case CO_NMT_CS_STOP: + case CO_NMT_CS_RESET_COMM: + default: + { + break; + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: main + ****************************************************************************/ + +int main(int argc, FAR char *argv[]) +{ + struct timespec now; + struct timespec t_last; + struct can_msg msg; + co_unsigned32_t counter = 0; + int ret; + + memset(&g_net, 0, sizeof(g_net)); + setvbuf(stdout, NULL, _IONBF, 0); + + ret = coslave_candev_init(); + if (ret < 0) + { + printf("ERROR: coslave_candev_init failed %d\n", ret); + goto errout; + } + + g_net.net = can_net_create(); + if (!g_net.net) + { + printf("ERROR: can_net_create failed\n"); + goto errout; + } + + can_net_set_send_func(g_net.net, &on_can_send, NULL); + + clock_gettime(CLOCK_MONOTONIC, &now); + can_net_set_time(g_net.net, &now); + + /* Create dev from struct */ + + g_net.dev = co_dev_create_from_sdev(&g_sdev_slave); + if (!g_net.dev) + { + printf("ERROR: co_dev_create_from_sdev failed\n"); + goto errout; + } + + g_net.nmt = co_nmt_create(g_net.net, g_net.dev); + if (!g_net.nmt) + { + printf("ERROR: co_nmt_create failed\n"); + goto errout; + } + + /* Start the NMT service by resetting the node */ + + co_nmt_cs_ind(g_net.nmt, CO_NMT_CS_RESET_NODE); + + /* Set the NMT indication function after the initial reset */ + + co_nmt_set_cs_ind(g_net.nmt, &on_nmt_cs, NULL); + + printf("lely CANopen slave running (node 0x%02x)\n", g_sdev_slave.id); + + clock_gettime(CLOCK_MONOTONIC, &t_last); + + while (1) + { + clock_gettime(CLOCK_MONOTONIC, &now); + can_net_set_time(g_net.net, &now); + + while (coslave_candev_recv(&msg) > 0) + { + can_net_recv(g_net.net, &msg); + } + + /* Update the counter mapped into TPDO1 (0x2100) every 100 ms. + * When the master has brought us to OPERATIONAL, the synchronous TPDO + * sends the current value on each SYNC. + */ + + if (elapsed_ms(&now, &t_last) >= 100) + { + t_last = now; + counter++; + co_dev_set_val_u32(g_net.dev, 0x2100, 0x00, counter); + } + + usleep(1000); + } + +errout: + if (g_net.nmt) + { + co_nmt_destroy(g_net.nmt); + } + + if (g_net.dev) + { + co_dev_destroy(g_net.dev); + } + + if (g_net.net) + { + can_net_destroy(g_net.net); + } + + return 0; +}
