diff --git a/sensors/.gitignore b/sensors/.gitignore new file mode 100644 index 00000000000..9e1d2593ee8 --- /dev/null +++ b/sensors/.gitignore @@ -0,0 +1 @@ +/Kconfig diff --git a/sensors/CMakeLists.txt b/sensors/CMakeLists.txt new file mode 100644 index 00000000000..28120de9c1e --- /dev/null +++ b/sensors/CMakeLists.txt @@ -0,0 +1,25 @@ +# ############################################################################## +# apps/sensors/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. +# +# ############################################################################## + +nuttx_add_subdirectory() + +nuttx_generate_kconfig(MENUDESC "Sensors") diff --git a/sensors/Make.defs b/sensors/Make.defs new file mode 100644 index 00000000000..be99375e05f --- /dev/null +++ b/sensors/Make.defs @@ -0,0 +1,23 @@ +############################################################################ +# apps/sensors/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. +# +############################################################################ + +include $(wildcard $(APPDIR)/sensors/*/Make.defs) diff --git a/sensors/Makefile b/sensors/Makefile new file mode 100644 index 00000000000..b75b05aa3ea --- /dev/null +++ b/sensors/Makefile @@ -0,0 +1,25 @@ +############################################################################ +# apps/sensors/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. +# +############################################################################ + +MENUDESC = "Sensors" + +include $(APPDIR)/Directory.mk diff --git a/sensors/netsensor/Kconfig b/sensors/netsensor/Kconfig new file mode 100644 index 00000000000..b0beb541970 --- /dev/null +++ b/sensors/netsensor/Kconfig @@ -0,0 +1,32 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config SENSORS_NETSENSOR + tristate "Netsensor" + default n + depends on UORB + depends on USENSOR + depends on NET_UDP + ---help--- + Program for a networked sensor that publishes data received over UDP as + uORB topics. + +if SENSORS_NETSENSOR + +comment "Program options" + +config SENSORS_NETSENSOR_PRIORITY + int "Priority" + default 100 + ---help--- + The task priority for the netsensor process. + +config SENSORS_NETSENSOR_STACKSIZE + int "Stack size" + default DEFAULT_TASK_STACKSIZE + ---help--- + The stack size for the netsensor process. + +endif # SENSORS_NETSENSOR diff --git a/sensors/netsensor/Make.defs b/sensors/netsensor/Make.defs new file mode 100644 index 00000000000..892449aee7b --- /dev/null +++ b/sensors/netsensor/Make.defs @@ -0,0 +1,23 @@ +############################################################################ +# apps/sensors/netsensor/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_SENSORS_NETSENSOR),) +CONFIGURED_APPS += $(APPDIR)/sensors/netsensor +endif diff --git a/sensors/netsensor/Makefile b/sensors/netsensor/Makefile new file mode 100644 index 00000000000..064afc41672 --- /dev/null +++ b/sensors/netsensor/Makefile @@ -0,0 +1,32 @@ +############################################################################ +# apps/sensors/netsensor/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 + +MODULE = $(CONFIG_SENSORS_NETSENSOR) +PRIORITY = $(CONFIG_SENSORS_NETSENSOR_PRIORITY) +STACKSIZE = $(CONFIG_SENSORS_NETSENSOR_STACKSIZE) +PROGNAME = netsensor + +# Topics to advertise + +MAINSRC = netsensor_main.c + +include $(APPDIR)/Application.mk diff --git a/sensors/netsensor/helptext.h b/sensors/netsensor/helptext.h new file mode 100644 index 00000000000..a32b8e7af7c --- /dev/null +++ b/sensors/netsensor/helptext.h @@ -0,0 +1,10 @@ +#define HELP_TEXT \ +"USAGE:\n netsensor [options] topic\n\nARGUMENTS:\n topic The " \ +"uORB this netsensor will publish.\n\nOPTIONS:\n -h Display th" \ +"is help message and quit.\n -p Specify the UDP port to read f" \ +"rom. Default: 5555\n -d Specify the device number that will b" \ +"e used for the new topic\n instance. Default: increment to" \ +" next available.\n -q Specify the queue length (buffer length" \ +") uORB will use to\n store published data. Default: 5\n " \ +" -t If this flag is passed, timestamps will be overwritten by th" \ +"e\n time the data was received over UDP.\n" diff --git a/sensors/netsensor/helptext.txt b/sensors/netsensor/helptext.txt new file mode 100644 index 00000000000..a8ac4bee4f9 --- /dev/null +++ b/sensors/netsensor/helptext.txt @@ -0,0 +1,15 @@ +USAGE: + netsensor [options] topic + +ARGUMENTS: + topic The uORB this netsensor will publish. + +OPTIONS: + -h Display this help message and quit. + -p Specify the UDP port to read from. Default: 5555 + -d Specify the device number that will be used for the new topic + instance. Default: increment to next available. + -q Specify the queue length (buffer length) uORB will use to + store published data. Default: 5 + -t If this flag is passed, timestamps will be overwritten by the + time the data was received over UDP. diff --git a/sensors/netsensor/netsensor_main.c b/sensors/netsensor/netsensor_main.c new file mode 100644 index 00000000000..4895f74997c --- /dev/null +++ b/sensors/netsensor/netsensor_main.c @@ -0,0 +1,246 @@ +/**************************************************************************** + * apps/sensors/netsensor/netsensor_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 asyslogditional 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 + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include "helptext.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define DEFAULT_PORT (5555) +#define DEFAULT_QLEN (5) + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void print_usage(FILE *sink) { fprintf(sink, HELP_TEXT); } + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int main(int argc, char **argv) +{ + int err; + int netfd; + int sock; + ssize_t brecvd; + const struct orb_metadata *meta; + struct sockaddr_in addr; /* TODO: IPv6 support */ + socklen_t addrlen = sizeof(addr); + int sockoptval; + void *dbuf = NULL; + + const char *topic = NULL; + uint16_t port = DEFAULT_PORT; + uint16_t queue_len = DEFAULT_QLEN; + int devno = 0; + bool usedevno = false; + bool restamp = false; + + /* Get parameters for operation from the command line. */ + + int c; + while ((c = getopt(argc, argv, ":p:q:d:th")) != -1) + { + switch (c) + { + case 'h': + print_usage(stdout); + return EXIT_SUCCESS; + break; + case 'p': + port = strtoul(optarg, NULL, 10); + break; + case 'd': + usedevno = true; + devno = atoi(optarg); + break; + case 'q': + queue_len = strtoul(optarg, NULL, 10); + break; + case 't': + restamp = true; + break; + case '?': + fprintf(stderr, "Unknown option -%c\n", optopt); + return EXIT_FAILURE; + break; + } + } + + /* Get topic name */ + + if (optind >= argc) + { + fprintf(stderr, "Missing topic.\n"); + print_usage(stderr); + return EXIT_FAILURE; + } + + topic = argv[optind]; + + /* Get topic metadata */ + + meta = orb_get_meta(topic); + if (meta == NULL) + { + fprintf(stderr, "Could not get metadata for topic '%s'\n", topic); + return EXIT_FAILURE; + } + + /* Set up topic advertisement */ + + if (usedevno) + { + netfd = orb_advertise_multi_queue(meta, NULL, &devno, queue_len); + } + else + { + netfd = orb_advertise_multi_queue(meta, NULL, NULL, queue_len); + } + + if (netfd < 0) + { + fprintf(stderr, "Could not advertise topic %s: %d\n", topic, errno); + return EXIT_FAILURE; + } + + /* Set up socket for receiving data */ + + addr.sin_family = AF_INET; + addr.sin_port = HTONS(port); + addr.sin_addr.s_addr = HTONL(INADDR_ANY); + + sock = socket(AF_INET, SOCK_DGRAM, 0); + if (sock < 0) + { + fprintf(stderr, "Couldn't create UDP socket: %d\n", errno); + err = EXIT_FAILURE; + goto cleanup_topic; + } + + if (bind(sock, (struct sockaddr *)&addr, addrlen) < 0) + { + fprintf(stderr, "Couldn't bind UDP socket to port %u: %d\n", port, + errno); + err = EXIT_FAILURE; + goto cleanup_sock; + } + + sockoptval = 1; + if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &sockoptval, + sizeof(sockoptval)) < 0) + { + fprintf(stderr, "setsockopt SO_REUSEADDR failure: %d\n", errno); + err = EXIT_FAILURE; + goto cleanup_sock; + } + + /* Allocate memory for receiving UDP messages containing data */ + + dbuf = malloc(meta->o_size); + if (dbuf == NULL) + { + syslog(LOG_ERR | LOG_DAEMON, + "Couldn't allocate recv buffer memory: %d\n", errno); + err = EXIT_FAILURE; + goto cleanup_sock; + } + + /* Receive and publish data forever */ + + syslog(LOG_INFO | LOG_DAEMON, "Starting netsensor %s on port %u\n", topic, + port); + for (;;) + { + brecvd = recv(sock, dbuf, meta->o_size, 0); + if (brecvd < 0) + { + syslog(LOG_ERR | LOG_DAEMON, "Could not receive data: %d\n", errno); + continue; + } + + if (brecvd != meta->o_size) + { + syslog(LOG_ERR | LOG_DAEMON, "Received incorrect data size.\n"); + continue; + } + + syslog(LOG_INFO | LOG_DAEMON, "Got data: %ld bytes\n", brecvd); + + /* If we're overwriting the timestamp, do that. We know that all uORB + * topics start with a `uint64_t timestamp` member. + */ + + if (restamp) + { + *((uint64_t *)(dbuf)) = orb_absolute_time(); + } + + /* Publish the data to the topic */ + + err = orb_publish(meta, netfd, dbuf); + if (err < 0) + { + syslog(LOG_ERR | LOG_DAEMON, "Could not publish data: %d\n", errno); + } + } + + free(dbuf); +cleanup_sock: + close(sock); +cleanup_topic: + orb_unadvertise(netfd); + return err; +} diff --git a/sensors/netsensor_stream/Kconfig b/sensors/netsensor_stream/Kconfig new file mode 100644 index 00000000000..f74d0e7a2f2 --- /dev/null +++ b/sensors/netsensor_stream/Kconfig @@ -0,0 +1,31 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config SENSORS_NETSENSOR_STREAM + tristate "Netsensor stream" + default n + depends on UORB + depends on NET_UDP + ---help--- + Program to stream uORB data over UDP in a format which can be + interpreted by the netsensor. + +if SENSORS_NETSENSOR_STREAM + +comment "Program options" + +config SENSORS_NETSENSOR_STREAM_PRIORITY + int "Priority" + default 100 + ---help--- + The task priority for the netsensor_stream process. + +config SENSORS_NETSENSOR_STREAM_STACKSIZE + int "Stack size" + default DEFAULT_TASK_STACKSIZE + ---help--- + The stack size for the netsensor_stream process. + +endif # SENSORS_NETSENSOR_STREAM diff --git a/sensors/netsensor_stream/Make.defs b/sensors/netsensor_stream/Make.defs new file mode 100644 index 00000000000..646f36261f7 --- /dev/null +++ b/sensors/netsensor_stream/Make.defs @@ -0,0 +1,23 @@ +############################################################################ +# apps/sensors/netsensor_stream/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_SENSORS_NETSENSOR_STREAM),) +CONFIGURED_APPS += $(APPDIR)/sensors/netsensor_stream +endif diff --git a/sensors/netsensor_stream/Makefile b/sensors/netsensor_stream/Makefile new file mode 100644 index 00000000000..513137c5208 --- /dev/null +++ b/sensors/netsensor_stream/Makefile @@ -0,0 +1,32 @@ +############################################################################ +# apps/sensors/netsensor_stream/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 + +MODULE = $(CONFIG_SENSORS_NETSENSOR_STREAM) +PRIORITY = $(CONFIG_SENSORS_NETSENSOR_STREAM_PRIORITY) +STACKSIZE = $(CONFIG_SENSORS_NETSENSOR_STREAM_STACKSIZE) +PROGNAME = netsensor_stream + +# Topics to advertise + +MAINSRC = netsensor_stream_main.c + +include $(APPDIR)/Application.mk diff --git a/sensors/netsensor_stream/helptext.h b/sensors/netsensor_stream/helptext.h new file mode 100644 index 00000000000..3f5d7f6f6f1 --- /dev/null +++ b/sensors/netsensor_stream/helptext.h @@ -0,0 +1,6 @@ +#define HELP_TEXT \ +"USAGE:\n netsensor_stream [options] topic\n\nARGUMENTS:\n topic " \ +" The uORB topic to stream (including devno). Ex: sensor_baro0\n\nOPTIONS:" \ +" -h Display this help message and quit.\n -a S" \ +"pecify the IP address to stream data to. Default: 127.0.0.1\n -p " \ +" Specify the UDP port to stream data to. Default: 5555\n" diff --git a/sensors/netsensor_stream/helptext.txt b/sensors/netsensor_stream/helptext.txt new file mode 100644 index 00000000000..e59c3db857a --- /dev/null +++ b/sensors/netsensor_stream/helptext.txt @@ -0,0 +1,10 @@ +USAGE: + netsensor_stream [options] topic + +ARGUMENTS: + topic The uORB topic to stream (including devno). Ex: sensor_baro0 + +OPTIONS: + -h Display this help message and quit. + -a Specify the IP address to stream data to. Default: 127.0.0.1 + -p Specify the UDP port to stream data to. Default: 5555 diff --git a/sensors/netsensor_stream/netsensor_stream_main.c b/sensors/netsensor_stream/netsensor_stream_main.c new file mode 100644 index 00000000000..3e974c60660 --- /dev/null +++ b/sensors/netsensor_stream/netsensor_stream_main.c @@ -0,0 +1,234 @@ +/**************************************************************************** + * apps/sensors/netsensor_stream/netsensor_stream_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 asyslogditional 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 + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include "helptext.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define DEFAULT_PORT (5555) +#define DEFAULT_IP "127.0.0.1" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void print_usage(FILE *sink) { fprintf(sink, HELP_TEXT); } + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int main(int argc, char **argv) +{ + int err; + int sock; + int fd; + ssize_t bsent; + struct orb_metadata meta; + struct pollfd pfd; + struct sockaddr_in addr; /* TODO: IPv6 support */ + struct sockaddr_in to; + socklen_t addrlen = sizeof(addr); + socklen_t tolen = sizeof(to); + void *dbuf = NULL; + + const char *topic = NULL; + uint16_t port = DEFAULT_PORT; + const char *ipaddr = DEFAULT_IP; + uint32_t ip; + + /* Get parameters for operation from the command line. */ + + int c; + while ((c = getopt(argc, argv, ":a:p:h")) != -1) + { + switch (c) + { + case 'h': + print_usage(stdout); + return EXIT_SUCCESS; + break; + case 'a': + ipaddr = optarg; + break; + case 'p': + port = strtoul(optarg, NULL, 10); + break; + case '?': + fprintf(stderr, "Unknown option -%c\n", optopt); + return EXIT_FAILURE; + break; + } + } + + /* Get topic name */ + + if (optind >= argc) + { + fprintf(stderr, "Missing topic.\n"); + print_usage(stderr); + return EXIT_FAILURE; + } + + topic = argv[optind]; + + /* Get topic metadata */ + + fd = orb_subscribe(&meta); + if (fd < 0) + { + fprintf(stderr, "Could not get subscribe to topic '%s'\n", topic); + return EXIT_FAILURE; + } + + /* Set up socket for sending data */ + + addr.sin_family = AF_INET; + addr.sin_port = 0; /* Allow any */ + addr.sin_addr.s_addr = HTONL(INADDR_ANY); + + /* Set up address where data is sent */ + + err = inet_pton(AF_INET, ipaddr, &ip); + if (err < 0) + { + fprintf(stderr, "IP address %s is invalid.\n", ipaddr); + err = EXIT_FAILURE; + goto cleanup_topic; + } + + to.sin_family = AF_INET; + to.sin_port = HTONL(port); /* Allow any */ + to.sin_addr.s_addr = (in_addr_t)ip; + + sock = socket(AF_INET, SOCK_DGRAM, 0); + if (sock < 0) + { + fprintf(stderr, "Couldn't create UDP socket: %d\n", errno); + err = EXIT_FAILURE; + goto cleanup_topic; + } + + if (bind(sock, (struct sockaddr *)&addr, addrlen) < 0) + { + fprintf(stderr, "Couldn't bind UDP socket to port %u: %d\n", port, + errno); + err = EXIT_FAILURE; + goto cleanup_sock; + } + + /* Allocate memory for receiving UDP messages containing data */ + + dbuf = malloc(meta.o_size); + if (dbuf == NULL) + { + syslog(LOG_ERR | LOG_DAEMON, + "Couldn't allocate send buffer memory: %d\n", errno); + err = EXIT_FAILURE; + goto cleanup_sock; + } + + /* Set up polling */ + + pfd.fd = fd; + pfd.events = POLLIN; + pfd.revents = 0; + + /* Send data from the topic forever */ + + syslog(LOG_INFO | LOG_DAEMON, "Streaming from %s to %s:%u\n", topic, ipaddr, + port); + + for (;;) + { + /* Get data from topic whenever it's ready (poll returns) */ + + err = poll(&pfd, 1, -1); + if (err <= 0) + { + syslog(LOG_ERR | LOG_DAEMON, "Failed to poll topic: %d\n", errno); + continue; /* Try again */ + } + + err = orb_copy(&meta, fd, dbuf); + if (err) + { + syslog(LOG_ERR | LOG_DAEMON, "Couldn't get topic data: %d\n", + errno); + continue; + } + + /* Send over UDP */ + + bsent = + sendto(sock, dbuf, meta.o_size, 0, (struct sockaddr *)&to, tolen); + if (bsent < 0) + { + syslog(LOG_ERR | LOG_DAEMON, "Couldn't send UDP data: %d\n", errno); + continue; + } + else if (bsent != meta.o_size) + { + syslog(LOG_ERR | LOG_DAEMON, "Sent UDP data of wrong size.\n"); + continue; + } + } + + free(dbuf); +cleanup_sock: + close(sock); +cleanup_topic: + orb_unsubscribe(fd); + return err; +}