diff options
author | Mark Charlebois <charlebm@gmail.com> | 2015-03-11 16:45:19 -0700 |
---|---|---|
committer | Mark Charlebois <charlebm@gmail.com> | 2015-04-20 11:00:16 -0700 |
commit | 5084a61f0ed5ed0910d3067c927c2787ee69a574 (patch) | |
tree | b12fe1acc98aa888fae641c139727257f79c7529 /src | |
parent | 89e1b454af22e67e559f0d7f99a81a826bf62bbc (diff) | |
download | px4-firmware-5084a61f0ed5ed0910d3067c927c2787ee69a574.tar.gz px4-firmware-5084a61f0ed5ed0910d3067c927c2787ee69a574.tar.bz2 px4-firmware-5084a61f0ed5ed0910d3067c927c2787ee69a574.zip |
Abstractions to compile systemlib for Linux and Nuttx
Modified uint32_t casts of pointers to unsigned long for portability.
It otherwise breaks on x86_64.
Added _PX4_IOC to handle the conflice between _IOC on Linux and NuttX.
Removed use of px4::ok() because it cannot be used in a thread based
implementation. Changed to use px4::AppMgr which uses ok() on ROS.
Removed up_cxxinitialize.c from Linux build.
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
Diffstat (limited to 'src')
23 files changed, 172 insertions, 114 deletions
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index e7ab9eb5a..4fddd072e 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -44,7 +44,7 @@ using namespace px4; PublisherExample::PublisherExample() : - _n(), + _n(mgr), _rc_channels_pub(_n.advertise<px4_rc_channels>()), _v_att_pub(_n.advertise<px4_vehicle_attitude>()), _parameter_update_pub(_n.advertise<px4_parameter_update>()) @@ -55,7 +55,7 @@ int PublisherExample::main() { px4::Rate loop_rate(10); - while (px4::ok()) { + while (!mgr.exitRequested()) { loop_rate.sleep(); _n.spinOnce(); diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 0a66d3edc..8b0d806bd 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -39,6 +39,7 @@ */ #pragma once #include <px4.h> +#include <px4_app.h> class PublisherExample { @@ -48,6 +49,8 @@ public: ~PublisherExample() {}; int main(); + + static px4::AppMgr mgr; protected: px4::NodeHandle _n; px4::Publisher<px4::px4_rc_channels> *_rc_channels_pub; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 5a23dc80b..91fb131f3 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -49,7 +49,7 @@ void rc_channels_callback_function(const px4_rc_channels &msg) } SubscriberExample::SubscriberExample() : - _n(), + _n(_mgr), _p_sub_interv("SUB_INTERV", PARAM_SUB_INTERV_DEFAULT), _p_test_float("SUB_TESTF", PARAM_SUB_TESTF_DEFAULT) { diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 9eb5dd2a0..8003525dd 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -51,12 +51,14 @@ public: ~SubscriberExample() {}; void spin() {_n.spin();} + protected: px4::NodeHandle _n; px4::ParameterInt _p_sub_interv; px4::ParameterFloat _p_test_float; px4::Subscriber<px4_rc_channels> *_sub_rc_chan; + AppMgr _mgr; void rc_channels_callback(const px4_rc_channels &msg); void vehicle_attitude_callback(const px4_vehicle_attitude &msg); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index b5a551109..34859071f 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -71,7 +71,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(nullptr), _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), - _n(), + _n(_mgr), /* parameters */ _params_handles({ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index 1242c7fe3..50f06d696 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -94,6 +94,8 @@ private: px4::NodeHandle _n; + px4::AppMgr _mgr; + struct { px4::ParameterFloat roll_p; px4::ParameterFloat roll_rate_p; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index d135eecfb..1fd86fd50 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -65,7 +65,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _local_pos_sp_msg(), _global_vel_sp_msg(), - _n(), + _n(_mgr), /* parameters */ _params_handles({ diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 54c6de155..3c6f28050 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -107,6 +107,8 @@ protected: px4::NodeHandle _n; + px4::AppMgr _mgr; + struct { px4::ParameterFloat thr_min; px4::ParameterFloat thr_max; diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 9540584a7..4f58ac71a 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -42,15 +42,15 @@ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> */ #include <px4_config.h> -#include <nuttx/sched.h> +//#include <nuttx/sched.h> #include <sys/types.h> #include <stdint.h> #include <stdbool.h> -#include <arch/arch.h> +//#include <arch/arch.h> -#include <debug.h> +//#include <debug.h> #include <sys/time.h> diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 515455f59..1f66971c1 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -43,6 +43,7 @@ #include "mcu_version.h" #include <px4_config.h> +#include <px4_defines.h> #ifdef CONFIG_ARCH_CHIP_STM32 #include <up_arch.h> diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f2499bbb1..5e35f2c2b 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -43,7 +43,6 @@ SRCS = err.c \ conversions.c \ cpuload.c \ getopt_long.c \ - up_cxxinitialize.c \ pid/pid.c \ systemlib.c \ airspeed.c \ @@ -57,6 +56,10 @@ SRCS = err.c \ circuit_breaker_params.c \ mcu_version.c +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS += up_cxxinitialize.c +endif + MAXOPTIMIZATION = -Os EXTRACFLAGS = -Wno-sign-compare diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 8d3257f23..cb8ffbd3c 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -170,7 +170,7 @@ void F_lock(void) } // flash write word. -int F_write_word(uint32_t Address, uint32_t Data) +int F_write_word(unsigned long Address, uint32_t Data) { unsigned char octet[4] = {0, 0, 0, 0}; @@ -185,7 +185,7 @@ int F_write_word(uint32_t Address, uint32_t Data) } // flash write byte -int F_write_byte(uint32_t Address, uint8_t Data) +int F_write_byte(unsigned long Address, uint8_t Data) { volatile int status = F_COMPLETE; diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index 273b064f0..3ea0f80bd 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -54,6 +54,7 @@ __BEGIN_DECLS #include <unistd.h> +#include <stdint.h> #include <stdio.h> // possible flash statuses @@ -64,30 +65,30 @@ __BEGIN_DECLS #define F_COMPLETE 5 typedef struct { - volatile uint32_t accesscontrol; // 0x00 - volatile uint32_t key; // 0x04 - volatile uint32_t optionkey; // 0x08 - volatile uint32_t status; // 0x0C - volatile uint32_t control; // 0x10 - volatile uint32_t optioncontrol; //0x14 + volatile unsigned long accesscontrol; // 0x00 + volatile unsigned long key; // 0x04 + volatile unsigned long optionkey; // 0x08 + volatile unsigned long status; // 0x0C + volatile unsigned long control; // 0x10 + volatile unsigned long optioncontrol; //0x14 } flash_registers; -#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address +#define PERIPH_BASE ((unsigned long)0x40000000) //Peripheral base address #define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000) #define F_R_BASE (AHB1PERIPH_BASE + 0x3C00) #define FLASH ((flash_registers *) F_R_BASE) -#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit -#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit -#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit -#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF) -#define F_PSIZE_WORD ((uint32_t)0x00000200) -#define F_PSIZE_BYTE ((uint32_t)0x00000000) -#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register -#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit. - -#define F_KEY1 ((uint32_t)0x45670123) -#define F_KEY2 ((uint32_t)0xCDEF89AB) +#define F_BSY ((unsigned long)0x00010000) //FLASH Busy flag bit +#define F_OPERR ((unsigned long)0x00000002) //FLASH operation Error flag bit +#define F_WRPERR ((unsigned long)0x00000010) //FLASH Write protected error flag bit +#define CR_PSIZE_MASK ((unsigned long)0xFFFFFCFF) +#define F_PSIZE_WORD ((unsigned long)0x00000200) +#define F_PSIZE_BYTE ((unsigned long)0x00000000) +#define F_CR_PG ((unsigned long)0x00000001) // a bit in the F_CR register +#define F_CR_LOCK ((unsigned long)0x80000000) // also another bit. + +#define F_KEY1 ((unsigned long)0x45670123) +#define F_KEY2 ((unsigned long)0xCDEF89AB) #define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) @@ -143,8 +144,8 @@ __EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signat __EXPORT int lock_otp(void); -__EXPORT int F_write_byte(uint32_t Address, uint8_t Data); -__EXPORT int F_write_word(uint32_t Address, uint32_t Data); +__EXPORT int F_write_byte(unsigned long Address, uint8_t Data); +__EXPORT int F_write_word(unsigned long Address, uint32_t Data); __END_DECLS diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 8dea6e6cc..dfbe582c5 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -41,7 +41,8 @@ * and background parameter saving. */ -#include <debug.h> +//#include <debug.h> +#include <px4_defines.h> #include <string.h> #include <stdbool.h> #include <fcntl.h> @@ -646,7 +647,7 @@ param_save_default(void) const char *filename = param_get_default_file(); /* write parameters to temp file */ - fd = open(filename, O_WRONLY | O_CREAT); + fd = open(filename, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("failed to open param file: %s", filename); diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 950577f00..909979eca 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -386,7 +386,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_COUNT: dprintf(fd, "%s: %llu events\n", handle->name, - ((struct perf_ctr_count *)handle)->event_count); + (unsigned long long)((struct perf_ctr_count *)handle)->event_count); break; case PC_ELAPSED: { @@ -395,12 +395,12 @@ perf_print_counter_fd(int fd, perf_counter_t handle) dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", handle->name, - pce->event_count, - pce->event_overruns, - pce->time_total, - pce->time_total / pce->event_count, - pce->time_least, - pce->time_most, + (unsigned long long)pce->event_count, + (unsigned long long)pce->event_overruns, + (unsigned long long)pce->time_total, + (unsigned long long)pce->time_total / pce->event_count, + (unsigned long long)pce->time_least, + (unsigned long long)pce->time_most, (double)(1e6f * rms)); break; } @@ -411,10 +411,10 @@ perf_print_counter_fd(int fd, perf_counter_t handle) dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", handle->name, - pci->event_count, - (pci->time_last - pci->time_first) / pci->event_count, - pci->time_least, - pci->time_most, + (unsigned long long)pci->event_count, + (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, + (unsigned long long)pci->time_least, + (unsigned long long)pci->time_most, (double)(1e6f * rms)); break; } diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index c84cffa18..f576736ab 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -40,6 +40,7 @@ #include <px4_config.h> #include <stdio.h> +#include <unistd.h> #include <fcntl.h> #include <systemlib/err.h> diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 73df1051d..b86e10773 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -37,78 +37,21 @@ * Implementation of commonly used low-level system-call like functions. */ -#include <px4_config.h> -#include <unistd.h> -#include <stdio.h> -#include <fcntl.h> -#include <sched.h> -#include <signal.h> -#include <unistd.h> -#include <float.h> -#include <string.h> - -#include <sys/stat.h> -#include <sys/types.h> - -#include <stm32_pwr.h> - +#include <px4_tasks.h> #include "systemlib.h" -// Didn't seem right to include up_internal.h, so direct extern instead. -extern void up_systemreset(void) noreturn_function; - void systemreset(bool to_bootloader) { - if (to_bootloader) { - stm32_pwr_enablebkp(); - - /* XXX wow, this is evil - write a magic number into backup register zero */ - *(uint32_t *)0x40002850 = 0xb007b007; - } - - up_systemreset(); - - /* lock up here */ - while (true); + px4_systemreset(to_bootloader); } -static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); - void killall() { -// printf("Sending SIGUSR1 to all processes now\n"); - - /* iterate through all tasks and send kill signal */ - sched_foreach(kill_task, NULL); -} - -static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) -{ - kill(tcb->pid, SIGUSR1); + px4_killall(); } -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) { - int pid; - - sched_lock(); - - /* create the task */ - pid = task_create(name, priority, stack_size, entry, argv); - - if (pid > 0) { - - /* configure the scheduler */ - struct sched_param param; - - param.sched_priority = priority; - sched_setscheduler(pid, scheduler, ¶m); - - /* XXX do any other private task accounting here before the task starts */ - } - - sched_unlock(); - - return pid; + return px4_task_spawn_cmd(name, scheduler, priority, stack_size, entry, argv); } diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index f3aca64c7..41707360e 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -39,10 +39,11 @@ ************************************************************************************/ #include <px4_config.h> +#include <stdint.h> -#include <debug.h> +//#include <debug.h> -#include <nuttx/arch.h> +//#include <nuttx/arch.h> //#include <arch/stm32/chip.h> //#include "chip.h" @@ -122,7 +123,8 @@ extern uint32_t _etext; * ***************************************************************************/ -__EXPORT void up_cxxinitialize(void) +__EXPORT void up_cxxinitialize(void); +void up_cxxinitialize(void) { initializer_t *initp; diff --git a/src/platforms/linux/include/crc32.h b/src/platforms/linux/include/crc32.h new file mode 100644 index 000000000..bf828e3e0 --- /dev/null +++ b/src/platforms/linux/include/crc32.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * include/crc.h + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __INCLUDE_CRC32_H +#define __INCLUDE_CRC32_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <sys/types.h> +#include <stdint.h> + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: crc32part + * + * Description: + * Continue CRC calculation on a part of the buffer. + * + ****************************************************************************/ + +EXTERN uint32_t crc32part(const uint8_t *src, size_t len, + uint32_t crc32val); + +/**************************************************************************** + * Name: crc32 + * + * Description: + * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' + * + ****************************************************************************/ + +EXTERN uint32_t crc32(const uint8_t *src, size_t len); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_CRC32_H */ diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h index 1314397b5..2c9765ae2 100644 --- a/src/platforms/px4_app.h +++ b/src/platforms/px4_app.h @@ -66,8 +66,6 @@ private: // Task/process based build #if defined(__PX4_ROS) || defined(__PX4_NUTTX) -#define PX4_MAIN main - // Thread based build #else diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 798443253..83bcb8882 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -42,4 +42,6 @@ #if defined(__PX4_NUTTX) #include <px4_config.h> +#elif defined (__PX4_LINUX) +#define CONFIG_NFILE_STREAMS 1 #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6cb7f3f42..ab41625d5 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -98,12 +98,22 @@ typedef param_t px4_param_t; (c) == '\r' || (c) == '\f' || c== '\v') #endif +#define _PX4_IOC(x,y) _IOC(x,y) + /* Linux Specific defines */ #elif defined(__PX4_LINUX) // Flag is meaningless on Linux #define O_BINARY 0 +// NuttX _IOC is equivalent to Linux _IO +#define _PX4_IOC(x,y) _IO(x,y) + +/* FIXME - Used to satisfy build */ +#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) + +#define getreg32(a) (*(volatile uint32_t *)(a)) + #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 83a3e422d..c45ce5cc7 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -42,6 +42,7 @@ #include "px4_subscriber.h" #include "px4_publisher.h" #include "px4_middleware.h" +#include "px4_app.h" #if defined(__PX4_ROS) /* includes when building for ros */ @@ -141,10 +142,11 @@ protected: class __EXPORT NodeHandle { public: - NodeHandle() : + NodeHandle(AppMgr &a) : _subs(), _pubs(), - _sub_min_interval(nullptr) + _sub_min_interval(nullptr), + _mgr(a) {} ~NodeHandle() @@ -262,7 +264,7 @@ public: */ void spin() { - while (ok()) { + while (!_mgr.exitRequested()) { const int timeout_ms = 100; /* Only continue in the loop if the nodehandle has subscriptions */ @@ -287,6 +289,8 @@ protected: SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ + AppMgr &_mgr; + /** * Check if this is the smallest interval so far and update _sub_min_interval */ |