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>

makefiles/linux/config_linux_default.mk

@@ -37,7 +37,7 @@
 #
 # Library modules
 #
-#MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib
 #MODULES		+= modules/systemlib/mixer
 #MODULES		+= modules/uORB
 #MODULES		+= modules/dataman

makefiles/setup.mk

@@ -33,8 +33,8 @@
 # Path and tool setup
 #
 
-#export PX4_TARGET_OS	 = nuttx
-export PX4_TARGET_OS	 = linux
+export PX4_TARGET_OS	 = nuttx
+#export PX4_TARGET_OS	 = linux
 
 #
 # Some useful paths.

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();
 

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;

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)
 {

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);

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({

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;

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({

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;

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>
 

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>

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

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;
 

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_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 ((uint32_t)0x45670123)
-#define F_KEY2 ((uint32_t)0xCDEF89AB)
+#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
 

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);

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;
 	}

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>

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);
+	px4_killall();
 }
 
-static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[])
 {
-	kill(tcb->pid, SIGUSR1);
-}
-
-int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, 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, &param);
-
-		/* 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);
 }

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;
 

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 */

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
 

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

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
 
 

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
 	 */