platforms/posix/src: move to platforms/posix/src/px4/common

platforms/posix/src/px4/common/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,3 +31,42 @@
 #
 ############################################################################
 
+
+add_subdirectory(px4_daemon)
+add_subdirectory(lockstep_scheduler)
+
+set(EXTRA_DEPENDS)
+if("${CONFIG_SHMEM}" STREQUAL "1")
+	list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon")
+	include(hexagon_sdk)
+	include_directories(${HEXAGON_SDK_INCLUDES})
+	include_directories(${PX4_BINARY_DIR}/platforms/posix)
+	list(APPEND SHMEM_SRCS
+		shmem_posix.cpp
+	)
+	add_definitions(-DCONFIG_SHMEM=1)
+	set(EXTRA_DEPENDS generate_px4muorb_stubs)
+endif()
+
+add_library(px4_layer
+	px4_posix_impl.cpp
+	px4_posix_tasks.cpp
+	px4_sem.cpp
+	px4_init.cpp
+	lib_crc32.c
+	drv_hrt.cpp
+	${SHMEM_SRCS}
+)
+target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
+target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable
+target_link_libraries(px4_layer PRIVATE work_queue px4_work_queue)
+target_link_libraries(px4_layer PRIVATE px4_daemon drivers_board)
+
+if(ENABLE_LOCKSTEP_SCHEDULER)
+	target_link_libraries(px4_layer PRIVATE lockstep_scheduler)
+endif()
+
+
+if (EXTRA_DEPENDS)
+	add_dependencies(px4_layer ${EXTRA_DEPENDS})
+endif()

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -0,0 +1,615 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012 - 2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_hrt.cpp
+ *
+ * High-resolution timer with callouts and timekeeping.
+ */
+
+#include <px4_time.h>
+#include <px4_posix.h>
+#include <px4_defines.h>
+#include <px4_workqueue.h>
+#include <px4_tasks.h>
+#include <drivers/drv_hrt.h>
+#include <semaphore.h>
+#include <time.h>
+#include <string.h>
+#include <errno.h>
+#include "hrt_work.h"
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+#include <lockstep_scheduler/lockstep_scheduler.h>
+#endif
+
+// Intervals in usec
+static constexpr unsigned HRT_INTERVAL_MIN = 50;
+static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
+
+static struct sq_queue_s	callout_queue;
+static px4_sem_t 	_hrt_lock;
+static struct work_s	_hrt_work;
+
+#ifndef __PX4_QURT
+static hrt_abstime px4_timestart_monotonic = 0;
+#else
+static int32_t dsp_offset = 0;
+#endif
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+static LockstepScheduler *lockstep_scheduler = new LockstepScheduler();
+#endif
+
+
+hrt_abstime hrt_absolute_time_offset();
+static void hrt_call_reschedule();
+static void hrt_call_invoke();
+__EXPORT hrt_abstime hrt_reset();
+
+hrt_abstime hrt_absolute_time_offset()
+{
+#ifndef __PX4_QURT
+	return px4_timestart_monotonic;
+#else
+	return 0;
+#endif
+}
+
+static void hrt_lock()
+{
+	px4_sem_wait(&_hrt_lock);
+}
+
+static void hrt_unlock()
+{
+	px4_sem_post(&_hrt_lock);
+}
+
+#if defined(__PX4_APPLE_LEGACY)
+#include <sys/time.h>
+
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	struct timeval now;
+	int rv = gettimeofday(&now, nullptr);
+
+	if (rv) {
+		return rv;
+	}
+
+	tp->tv_sec = now.tv_sec;
+	tp->tv_nsec = now.tv_usec * 1000;
+
+	return 0;
+}
+
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+{
+	/* do nothing right now */
+	return 0;
+}
+
+#elif defined(__PX4_QURT)
+
+#include "dspal_time.h"
+
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+{
+	/* do nothing right now */
+	return 0;
+}
+
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	return clock_gettime(clk_id, tp);
+}
+
+#endif
+
+#ifndef __PX4_QURT
+/*
+ * Get system time in us
+ */
+uint64_t hrt_system_time()
+{
+	struct timespec ts;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
+}
+#endif
+
+/*
+ * Get absolute time.
+ */
+hrt_abstime hrt_absolute_time()
+{
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	// optimized case (avoid ts_to_abstime) if lockstep scheduler is used
+	const uint64_t abstime = lockstep_scheduler->get_absolute_time();
+	return abstime - px4_timestart_monotonic;
+#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
+	struct timespec ts;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+#ifdef __PX4_QURT
+	return ts_to_abstime(&ts) + dsp_offset;
+#else
+	return ts_to_abstime(&ts);
+#endif
+#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
+}
+
+#ifdef __PX4_QURT
+int hrt_set_absolute_time_offset(int32_t time_diff_us)
+{
+	dsp_offset = time_diff_us;
+	return 0;
+}
+#endif
+
+hrt_abstime hrt_reset()
+{
+#ifndef __PX4_QURT
+	px4_timestart_monotonic = 0;
+#endif
+	return hrt_absolute_time();
+}
+
+/*
+ * Convert a timespec to absolute time.
+ */
+hrt_abstime ts_to_abstime(const struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/*
+ * Compute the delta between a timestamp taken in the past
+ * and now.
+ *
+ * This function is safe to use even if the timestamp is updated
+ * by an interrupt during execution.
+ */
+hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	// This is not atomic as the value on the application layer of POSIX is limited.
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
+}
+
+/*
+ * Store the absolute time in an interrupt-safe fashion.
+ *
+ * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
+ */
+hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
+{
+	hrt_abstime ts = hrt_absolute_time();
+	return ts;
+}
+
+
+/*
+ * If this returns true, the entry has been invoked and removed from the callout list,
+ * or it has never been entered.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool	hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+/*
+ * Remove the entry from the callout list.
+ */
+void	hrt_cancel(struct hrt_call *entry)
+{
+	hrt_lock();
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+	hrt_unlock();
+	// endif
+}
+
+/*
+ * initialise a hrt_call structure
+ */
+void	hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+/*
+ * delay a hrt_call_every() periodic call by the given number of
+ * microseconds. This should be called from within the callout to
+ * cause the callout to be re-scheduled for a later time. The periodic
+ * callouts will then continue from that new base time at the
+ * previously specified period.
+ */
+void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
+
+/*
+ * Initialise the HRT.
+ */
+void	hrt_init()
+{
+	sq_init(&callout_queue);
+
+	int sem_ret = px4_sem_init(&_hrt_lock, 0, 1);
+
+	if (sem_ret) {
+		PX4_ERR("SEM INIT FAIL: %s", strerror(errno));
+	}
+
+	memset(&_hrt_work, 0, sizeof(_hrt_work));
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	//PX4_INFO("hrt_call_enter");
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == nullptr) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//if (call != nullptr) PX4_INFO("call enter at head, reschedule (%lu %lu)", entry->deadline, call->deadline);
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == nullptr) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != nullptr);
+	}
+
+	//PX4_INFO("scheduled");
+}
+
+/**
+ * Timer interrupt handler
+ *
+ * This routine simulates a timer interrupt handler
+ */
+static void
+hrt_tim_isr(void *p)
+{
+
+	//PX4_INFO("hrt_tim_isr");
+	/* run any callouts that have met their deadline */
+	hrt_call_invoke();
+
+	hrt_lock();
+
+	/* and schedule the next interrupt */
+	hrt_call_reschedule();
+
+	hrt_unlock();
+}
+
+/**
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	hrt_abstime	delay = HRT_INTERVAL_MAX;
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	//PX4_INFO("hrt_call_reschedule");
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != nullptr) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			delay = HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			delay = next->deadline - now;
+		}
+	}
+
+	// There is no timer ISR, so simulate one by putting an event on the
+	// high priority work queue
+
+	// Remove the existing expiry and update with the new expiry
+	hrt_work_cancel(&_hrt_work);
+
+	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
+	hrt_lock();
+
+	//PX4_INFO("hrt_call_internal after lock");
+	/* if the entry is currently queued, remove it */
+	/* note that we are using a potentially uninitialised
+	   entry->link here, but it is safe as sq_rem() doesn't
+	   dereference the passed node unless it is found in the
+	   list. So we potentially waste a bit of time searching the
+	   queue for the uninitialised entry->link but we don't do
+	   anything actually unsafe.
+	*/
+	if (entry->deadline != 0) {
+		sq_rem(&entry->link, &callout_queue);
+	}
+
+#if 1
+
+	// Use this to debug busy CPU that keeps rescheduling with 0 period time
+	/*if (interval < HRT_INTERVAL_MIN) {*/
+	/*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/
+	/*abort();*/
+	/*}*/
+
+#endif
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+	hrt_unlock();
+}
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is nullptr, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	//printf("hrt_call_after\n");
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+/*
+ * Convert absolute time to a timespec.
+ */
+void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+
+static void
+hrt_call_invoke()
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	hrt_lock();
+
+	while (true) {
+		/* get the current time */
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == nullptr) {
+			break;
+		}
+
+		if (call->deadline > now) {
+			break;
+		}
+
+		sq_rem(&call->link, &callout_queue);
+		//PX4_INFO("call pop");
+
+		/* save the intended deadline for periodic calls */
+		deadline = call->deadline;
+
+		/* zero the deadline, as the call has occurred */
+		call->deadline = 0;
+
+		/* invoke the callout (if there is one) */
+		if (call->callout) {
+			// Unlock so we don't deadlock in callback
+			hrt_unlock();
+
+			//PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg);
+			call->callout(call->arg);
+
+			hrt_lock();
+		}
+
+		/* if the callout has a non-zero period, it has to be re-entered */
+		if (call->period != 0) {
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+				//PX4_INFO("call deadline set to %lu now=%lu", call->deadline,  now);
+			}
+
+			hrt_call_enter(call);
+		}
+	}
+
+	hrt_unlock();
+}
+
+void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+#if !defined(__PX4_QURT)
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	if (clk_id == CLOCK_MONOTONIC) {
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+		const uint64_t abstime = lockstep_scheduler->get_absolute_time();
+		abstime_to_ts(tp, abstime - px4_timestart_monotonic);
+		return 0;
+#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
+#if defined(__PX4_DARWIN)
+		// We don't have CLOCK_MONOTONIC on macOS, so we just have to
+		// resort back to CLOCK_REALTIME here.
+		return system_clock_gettime(CLOCK_REALTIME, tp);
+#else // defined(__PX4_DARWIN)
+		return system_clock_gettime(clk_id, tp);
+#endif // defined(__PX4_DARWIN)
+#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+	} else {
+		return system_clock_gettime(clk_id, tp);
+	}
+}
+#endif // !defined(__PX4_QURT)
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
+{
+	if (clk_id == CLOCK_REALTIME) {
+		return system_clock_settime(clk_id, ts);
+
+	} else {
+		const uint64_t time_us = ts_to_abstime(ts);
+
+		if (px4_timestart_monotonic == 0) {
+			px4_timestart_monotonic = time_us;
+		}
+
+		lockstep_scheduler->set_absolute_time(time_us);
+		return 0;
+	}
+}
+
+
+int px4_usleep(useconds_t usec)
+{
+	if (px4_timestart_monotonic == 0) {
+		// Until the time is set by the simulator, we fallback to the normal
+		// usleep;
+		return system_usleep(usec);
+	}
+
+	const uint64_t time_finished = lockstep_scheduler->get_absolute_time() + usec;
+
+	return lockstep_scheduler->usleep_until(time_finished);
+}
+
+unsigned int px4_sleep(unsigned int seconds)
+{
+	if (px4_timestart_monotonic == 0) {
+		// Until the time is set by the simulator, we fallback to the normal
+		// sleep;
+		return system_sleep(seconds);
+	}
+
+	const uint64_t time_finished = lockstep_scheduler->get_absolute_time() +
+				       ((uint64_t)seconds * 1000000);
+
+	return lockstep_scheduler->usleep_until(time_finished);
+}
+
+int px4_pthread_cond_timedwait(pthread_cond_t *cond,
+			       pthread_mutex_t *mutex,
+			       const struct timespec *ts)
+{
+	const uint64_t time_us = ts_to_abstime(ts);
+	const uint64_t scheduled = time_us + px4_timestart_monotonic;
+	return lockstep_scheduler->cond_timedwait(cond, mutex, scheduled);
+}
+#endif

platforms/posix/src/px4/common/lib_crc32.c

@@ -0,0 +1,125 @@
+/************************************************************************************************
+ * libc/misc/lib_crc32.c
+ *
+ * This file is a part of NuttX:
+ *
+ *   Copyright (C) 2010-2011 Gregory Nutt. All rights reserved.
+ *
+ * The logic in this file was developed by Gary S. Brown:
+ *
+ *   COPYRIGHT (C) 1986 Gary S. Brown.  You may use this program, or code or tables
+ *   extracted from it, as desired without restriction.
+ *
+ * First, the polynomial itself and its table of feedback terms.  The polynomial is:
+ *
+ *    X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0
+ *
+ * Note that we take it "backwards" and put the highest-order term in the lowest-order bit.
+ * The X^32 term is "implied"; the LSB is the X^31 term, etc.  The X^0 term (usually shown
+ * as "+1") results in the MSB being 1
+ *
+ * Note that the usual hardware shift register implementation, which is what we're using
+ * (we're merely optimizing it by doing eight-bit chunks at a time) shifts bits into the
+ * lowest-order term.  In our implementation, that means shifting towards the right.  Why
+ * do we do it this way?  Because the calculated CRC must be transmitted in order from
+ * highest-order term to lowest-order term.  UARTs transmit characters in order from LSB
+ * to MSB.  By storing the CRC this way we hand it to the UART in the order low-byte to
+ * high-byte; the UART sends each low-bit to hight-bit; and the result is transmission bit
+ * by bit from highest- to lowest-order term without requiring any bit shuffling on our
+ * part.  Reception works similarly
+ *
+ * The feedback terms table consists of 256, 32-bit entries.  Notes
+ *
+ * - The table can be generated at runtime if desired; code to do so is shown later.  It
+ *   might not be obvious, but the feedback terms simply represent the results of eight
+ *   shift/xor operations for all combinations of data and CRC register values
+ *
+ * - The values must be right-shifted by eight bits by the updcrc logic; the shift must
+ *   be u_(bring in zeroes).  On some hardware you could probably optimize the shift in
+ *   assembler by using byte-swap instructions polynomial $edb88320
+  ************************************************************************************************/
+
+/************************************************************************************************
+ * Included Files
+ ************************************************************************************************/
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <crc32.h>
+
+// Needed for Linux
+#define FAR
+
+/************************************************************************************************
+ * Private Data
+ ************************************************************************************************/
+
+static const uint32_t crc32_tab[] = {
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+/************************************************************************************************
+ * Public Functions
+ ************************************************************************************************/
+/************************************************************************************************
+ * Name: crc32part
+ *
+ * Description:
+ *   Continue CRC calculation on a part of the buffer.
+ *
+ ************************************************************************************************/
+
+uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val)
+{
+	size_t i;
+
+	for (i = 0;  i < len;  i++) {
+		crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8);
+	}
+
+	return crc32val;
+}
+
+/************************************************************************************************
+ * Name: crc32
+ *
+ * Description:
+ *   Return a 32-bit CRC of the contents of the 'src' buffer, length 'len'
+ *
+ ************************************************************************************************/
+
+uint32_t crc32(FAR const uint8_t *src, size_t len)
+{
+	return crc32part(src, len, 0);
+}

platforms/posix/src/px4/common/lockstep_scheduler/.gitignore

@@ -0,0 +1 @@
+build/

platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+# We want to test the lockstep schedule even if it is not used otherwise.
+add_library(lockstep_scheduler
+	src/lockstep_scheduler.cpp
+)
+
+target_include_directories(lockstep_scheduler
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+px4_add_unit_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler)

platforms/posix/src/px4/common/lockstep_scheduler/build-and-test.sh

@@ -0,0 +1,11 @@
+#!/usr/bin/env bash
+
+set -e
+
+rm -rf build
+
+(mkdir -p build && cd build && CXX=g++ cmake .. && make -j8 && time $@ ./test/lockstep_scheduler_test)
+rm -rf build
+
+(mkdir -p build && cd build && CXX=clang++ cmake .. && make -j8 && time $@ ./test/lockstep_scheduler_test)
+rm -rf build

platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <cstdint>
+#include <mutex>
+#include <vector>
+#include <memory>
+#include <atomic>
+#include <pthread.h>
+
+class LockstepScheduler
+{
+public:
+	~LockstepScheduler();
+
+	void set_absolute_time(uint64_t time_us);
+	inline uint64_t get_absolute_time() const { return _time_us; }
+	int cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *lock, uint64_t time_us);
+	int usleep_until(uint64_t timed_us);
+
+private:
+	struct TimedWait {
+		~TimedWait()
+		{
+			if (!done) {
+				// This can only happen when a thread gets canceled (e.g. via pthread_cancel), and since
+				// pthread_cond_wait is a cancellation point, the rest of LockstepScheduler::cond_timedwait afterwards
+				// might not be executed. Which means the mutex will not be unlocked either, so we unlock to avoid
+				// a dead-lock in LockstepScheduler::set_absolute_time().
+				// This destructor gets called as part of thread-local storage cleanup.
+				// This is really only a work-around for non-proper thread stopping. Note that we also assume,
+				// that we can still access the mutex.
+				if (passed_lock) {
+					pthread_mutex_unlock(passed_lock);
+				}
+
+				done = true;
+			}
+
+			// If a thread quickly exits after a cond_timedwait(), the
+			// thread_local object can still be in the linked list. In that case
+			// we need to wait until it's removed.
+			while (!removed) {
+				system_usleep(5000);
+			}
+		}
+
+		pthread_cond_t *passed_cond{nullptr};
+		pthread_mutex_t *passed_lock{nullptr};
+		uint64_t time_us{0};
+		bool timeout{false};
+		std::atomic<bool> done{false};
+		std::atomic<bool> removed{true};
+
+		TimedWait *next{nullptr}; ///< linked list
+	};
+
+	std::atomic<uint64_t> _time_us{0};
+
+	TimedWait *_timed_waits{nullptr}; ///< head of linked list
+	std::mutex _timed_waits_mutex;
+	std::atomic<bool> _setting_time{false}; ///< true if set_absolute_time() is currently being executed
+};

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp

@@ -0,0 +1,134 @@
+#include <lockstep_scheduler/lockstep_scheduler.h>
+
+LockstepScheduler::~LockstepScheduler()
+{
+	// cleanup the linked list
+	std::unique_lock<std::mutex> lock_timed_waits(_timed_waits_mutex);
+
+	while (_timed_waits) {
+		TimedWait *tmp = _timed_waits;
+		_timed_waits = _timed_waits->next;
+		tmp->removed = true;
+	}
+}
+
+void LockstepScheduler::set_absolute_time(uint64_t time_us)
+{
+	_time_us = time_us;
+
+	{
+		std::unique_lock<std::mutex> lock_timed_waits(_timed_waits_mutex);
+		_setting_time = true;
+
+		TimedWait *timed_wait = _timed_waits;
+		TimedWait *timed_wait_prev = nullptr;
+
+		while (timed_wait) {
+			// Clean up the ones that are already done from last iteration.
+			if (timed_wait->done) {
+				// Erase from the linked list
+				if (timed_wait_prev) {
+					timed_wait_prev->next = timed_wait->next;
+
+				} else {
+					_timed_waits = timed_wait->next;
+				}
+
+				TimedWait *tmp = timed_wait;
+				timed_wait = timed_wait->next;
+				tmp->removed = true;
+				continue;
+			}
+
+			if (timed_wait->time_us <= time_us &&
+			    !timed_wait->timeout) {
+				// We are abusing the condition here to signal that the time
+				// has passed.
+				pthread_mutex_lock(timed_wait->passed_lock);
+				timed_wait->timeout = true;
+				pthread_cond_broadcast(timed_wait->passed_cond);
+				pthread_mutex_unlock(timed_wait->passed_lock);
+			}
+
+			timed_wait_prev = timed_wait;
+			timed_wait = timed_wait->next;
+		}
+
+		_setting_time = false;
+	}
+}
+
+int LockstepScheduler::cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *lock, uint64_t time_us)
+{
+	// A TimedWait object might still be in timed_waits_ after we return, so its lifetime needs to be
+	// longer. And using thread_local is more efficient than malloc.
+	static thread_local TimedWait timed_wait;
+	{
+		std::lock_guard<std::mutex> lock_timed_waits(_timed_waits_mutex);
+
+		// The time has already passed.
+		if (time_us <= _time_us) {
+			return ETIMEDOUT;
+		}
+
+		timed_wait.time_us = time_us;
+		timed_wait.passed_cond = cond;
+		timed_wait.passed_lock = lock;
+		timed_wait.timeout = false;
+		timed_wait.done = false;
+
+		// Add to linked list if not removed yet (otherwise just re-use the object)
+		if (timed_wait.removed) {
+			timed_wait.removed = false;
+			timed_wait.next = _timed_waits;
+			_timed_waits = &timed_wait;
+		}
+	}
+
+	int result = pthread_cond_wait(cond, lock);
+
+	const bool timeout = timed_wait.timeout;
+
+	if (result == 0 && timeout) {
+		result = ETIMEDOUT;
+	}
+
+	timed_wait.done = true;
+
+	if (!timeout && _setting_time) {
+		// This is where it gets tricky: the timeout has not been triggered yet,
+		// and another thread is in set_absolute_time().
+		// If it already passed the 'done' check, it will access the mutex and
+		// the condition variable next. However they might be invalid as soon as we
+		// return here, so we wait until set_absolute_time() is done.
+		// In addition we have to unlock 'lock', otherwise we risk a
+		// deadlock due to a different locking order in set_absolute_time().
+		// Note that this case does not happen too frequently, and thus can be
+		// a bit more expensive.
+		pthread_mutex_unlock(lock);
+		_timed_waits_mutex.lock();
+		_timed_waits_mutex.unlock();
+		pthread_mutex_lock(lock);
+	}
+
+	return result;
+}
+
+int LockstepScheduler::usleep_until(uint64_t time_us)
+{
+	pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
+	pthread_cond_t cond = PTHREAD_COND_INITIALIZER;
+
+	pthread_mutex_lock(&lock);
+
+	int result = cond_timedwait(&cond, &lock, time_us);
+
+	if (result == ETIMEDOUT) {
+		// This is expected because we never notified to the condition.
+		result = 0;
+	}
+
+	pthread_mutex_unlock(&lock);
+
+	return result;
+}

platforms/posix/src/px4/common/lockstep_scheduler/test/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+add_executable(lockstep_scheduler_test
+    src/lockstep_scheduler_test.cpp
+)
+
+target_link_libraries(lockstep_scheduler_test
+    lockstep_scheduler
+)
+
+target_compile_options(lockstep_scheduler_test PRIVATE -Wall -Wextra -Werror -O2)

platforms/posix/src/px4/common/lockstep_scheduler/test/src/lockstep_scheduler_test.cpp

@@ -0,0 +1,320 @@
+#include <lockstep_scheduler/lockstep_scheduler.h>
+#include <gtest/gtest.h>
+#include <thread>
+#include <atomic>
+#include <random>
+#include <iostream>
+#include <functional>
+#include <chrono>
+
+class TestThread
+{
+public:
+	TestThread(const std::function<void()> &f)
+		: _f(f)
+	{
+		_thread = std::thread(std::bind(&TestThread::execute, this));
+	}
+
+	void join(LockstepScheduler &ls)
+	{
+		// The unit-tests do not reflect the real usage, where
+		// set_absolute_time() is called regularly and can do some
+		// cleanup tasks. We simulate that here by waiting until
+		// the given task returns (which is expected to happen quickly)
+		// and then call set_absolute_time(), which can do the cleanup,
+		// and _thread can then exit as well.
+		while (!_done) {
+			std::this_thread::yield(); // usleep is too slow here
+		}
+
+		ls.set_absolute_time(ls.get_absolute_time());
+		_thread.join();
+	}
+private:
+	void execute()
+	{
+		_f();
+		_done = true;
+	}
+	std::function<void()> _f;
+	std::atomic<bool> _done{false};
+	std::thread _thread;
+};
+
+constexpr uint64_t some_time_us = 12345678;
+
+void test_absolute_time()
+{
+	LockstepScheduler ls;
+	ls.set_absolute_time(some_time_us);
+	EXPECT_EQ(ls.get_absolute_time(),  some_time_us);
+}
+
+void test_condition_timing_out()
+{
+	// Create locked condition.
+	pthread_cond_t cond;
+	pthread_cond_init(&cond, NULL);
+
+	// And a lock which needs to be locked
+	pthread_mutex_t lock;
+	pthread_mutex_init(&lock, NULL);
+
+	LockstepScheduler ls;
+	ls.set_absolute_time(some_time_us);
+
+	std::atomic<bool> should_have_timed_out{false};
+	pthread_mutex_lock(&lock);
+
+	// Use a thread to wait for condition while we already have the lock.
+	// This ensures the synchronization happens in the right order.
+	TestThread thread([&ls, &cond, &lock, &should_have_timed_out]() {
+		EXPECT_EQ(ls.cond_timedwait(&cond, &lock, some_time_us + 1000), ETIMEDOUT);
+		EXPECT_TRUE(should_have_timed_out);
+		// It should be re-locked afterwards, so we should be able to unlock it.
+		EXPECT_EQ(pthread_mutex_unlock(&lock), 0);
+	});
+
+	ls.set_absolute_time(some_time_us + 500);
+	should_have_timed_out = true;
+	ls.set_absolute_time(some_time_us + 1500);
+
+	thread.join(ls);
+
+	pthread_mutex_destroy(&lock);
+	pthread_cond_destroy(&cond);
+
+}
+
+void test_locked_semaphore_getting_unlocked()
+{
+	// Create locked condition.
+	pthread_cond_t cond;
+	pthread_cond_init(&cond, NULL);
+
+	// And a lock which needs to be locked
+	pthread_mutex_t lock;
+	pthread_mutex_init(&lock, NULL);
+
+	LockstepScheduler ls;
+	ls.set_absolute_time(some_time_us);
+
+	pthread_mutex_lock(&lock);
+	// Use a thread to wait for condition while we already have the lock.
+	// This ensures the synchronization happens in the right order.
+	TestThread thread([&ls, &cond, &lock]() {
+
+		ls.set_absolute_time(some_time_us + 500);
+		EXPECT_EQ(ls.cond_timedwait(&cond, &lock, some_time_us + 1000), 0);
+		// It should be re-locked afterwards, so we should be able to unlock it.
+		EXPECT_EQ(pthread_mutex_unlock(&lock), 0);
+	});
+
+	pthread_mutex_lock(&lock);
+	pthread_cond_broadcast(&cond);
+	pthread_mutex_unlock(&lock);
+
+	thread.join(ls);
+
+	pthread_mutex_destroy(&lock);
+	pthread_cond_destroy(&cond);
+}
+
+class TestCase
+{
+public:
+	TestCase(unsigned timeout, unsigned unlocked_after, LockstepScheduler &ls) :
+		_timeout(timeout + some_time_us),
+		_unlocked_after(unlocked_after + some_time_us),
+		_ls(ls)
+	{
+		pthread_mutex_init(&_lock, NULL);
+		pthread_cond_init(&_cond, NULL);
+	}
+
+	~TestCase()
+	{
+		EXPECT_TRUE(_is_done);
+		pthread_mutex_destroy(&_lock);
+		pthread_cond_destroy(&_cond);
+	}
+
+	void run()
+	{
+		pthread_mutex_lock(&_lock);
+		_thread = std::make_shared<TestThread>([this]() {
+			_result = _ls.cond_timedwait(&_cond, &_lock, _timeout);
+			pthread_mutex_unlock(&_lock);
+		});
+	}
+
+	void check()
+	{
+		if (_is_done) {
+			return;
+		}
+
+		uint64_t time_us = _ls.get_absolute_time();
+
+		const bool unlock_reached = (time_us >= _unlocked_after);
+		const bool unlock_is_before_timeout = (_unlocked_after <= _timeout);
+		const bool timeout_reached = (time_us >= _timeout);
+
+		if (unlock_reached && unlock_is_before_timeout && !(timeout_reached)) {
+			pthread_mutex_lock(&_lock);
+			pthread_cond_broadcast(&_cond);
+			pthread_mutex_unlock(&_lock);
+			_is_done = true;
+			// We can be sure that this triggers.
+			_thread->join(_ls);
+			EXPECT_EQ(_result, 0);
+		}
+
+		else if (timeout_reached) {
+			_is_done = true;
+			_thread->join(_ls);
+			EXPECT_EQ(_result, ETIMEDOUT);
+		}
+	}
+private:
+	static constexpr int INITIAL_RESULT = 42;
+
+	unsigned _timeout;
+	unsigned _unlocked_after;
+	pthread_cond_t _cond;
+	pthread_mutex_t _lock;
+	LockstepScheduler &_ls;
+	std::atomic<bool> _is_done{false};
+	std::atomic<int> _result {INITIAL_RESULT};
+	std::shared_ptr<TestThread> _thread{};
+};
+
+int random_number(int min, int max)
+{
+	// We want predictable test results, so we always
+	// start with the seed 0.
+	static int iteration = 0;
+
+	std::seed_seq seed{iteration++};
+	std::default_random_engine engine{seed};
+	std::uniform_int_distribution<> distribution(min, max);
+
+	const int random_number = distribution(engine);
+	return random_number;
+}
+
+void test_multiple_semaphores_waiting()
+{
+
+	LockstepScheduler ls;
+	ls.set_absolute_time(some_time_us);
+
+	// Use different timeouts in random order.
+	std::vector<std::shared_ptr<TestCase>> test_cases{};
+
+	test_cases.push_back(
+		std::make_shared<TestCase>(
+			11111, 11111, ls));
+
+	test_cases.push_back(
+		std::make_shared<TestCase>(
+			20000, 20000, ls));
+
+	test_cases.push_back(
+		std::make_shared<TestCase>(
+			0, 20000, ls));
+
+	test_cases.push_back(
+		std::make_shared<TestCase>(
+			20000, 10000, ls));
+
+	test_cases.push_back(
+		std::make_shared<TestCase>(
+			0, 0, ls));
+
+	const int num_additional_threads = random_number(1, 20);
+
+	for (int i = 0; i < num_additional_threads; ++i) {
+		const unsigned timeout = random_number(1, 20000);
+		const unsigned unlocked_after = random_number(1, 20000);
+		test_cases.push_back(
+			std::make_shared<TestCase>(
+				timeout, unlocked_after, ls));
+	}
+
+	for (auto &test_case : test_cases) {
+		test_case->run();
+	}
+
+	const int min_step_size = 1;
+	const int max_step_size = 100;
+
+	// We need to go until the max plus max step size to make sure we trigger
+	// all timeouts or semaphores.
+	for (unsigned time_us = 1;
+	     time_us <= (20000 + max_step_size);
+	     time_us += random_number(min_step_size, max_step_size)) {
+
+		ls.set_absolute_time(some_time_us + time_us);
+
+		for (auto &test_case : test_cases) {
+			test_case->check();
+		}
+	}
+
+	test_cases.clear();
+}
+
+#define WAIT_FOR(condition_) \
+	while (!(condition_)) { \
+		std::this_thread::yield(); \
+	}
+
+void test_usleep()
+{
+	LockstepScheduler ls;
+	ls.set_absolute_time(some_time_us);
+
+	enum class Step {
+		Init,
+		ThreadStarted,
+		BeforeUsleep,
+		UsleepNotTriggeredYet,
+		UsleepTriggered
+	};
+
+	std::atomic<Step> step{Step::Init};
+
+	TestThread thread([&step, &ls]() {
+		step = Step::ThreadStarted;
+
+		WAIT_FOR(step == Step::BeforeUsleep);
+
+		step = Step::UsleepNotTriggeredYet;
+		ls.set_absolute_time(some_time_us + 500);
+
+		step = Step::UsleepTriggered;
+		ls.set_absolute_time(some_time_us + 1500);
+	});
+
+	WAIT_FOR(step == Step::ThreadStarted);
+
+	step = Step::BeforeUsleep;
+
+	EXPECT_EQ(ls.usleep_until(some_time_us + 1000), 0);
+	EXPECT_EQ(step, Step::UsleepTriggered);
+	thread.join(ls);
+}
+
+TEST(LockstepScheduler, All)
+{
+	for (unsigned iteration = 1; iteration <= 100; ++iteration) {
+		//std::cout << "Test iteration: " << iteration << "\n";
+		test_absolute_time();
+		test_condition_timing_out();
+		test_locked_semaphore_getting_unlocked();
+		test_usleep();
+		test_multiple_semaphores_waiting();
+	}
+}

platforms/posix/src/px4/common/main.cpp

@@ -0,0 +1,665 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015-2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file main.cpp
+ *
+ * This is the main() of PX4 for POSIX.
+ *
+ * The application is designed as a daemon/server app with multiple clients.
+ * Both, the server and the client is started using this main() function.
+ *
+ * If the executable is called with its usual name 'px4', it will start the
+ * server. However, if it is started with an executable name (symlink) starting
+ * with 'px4-' such as 'px4-navigator', it will start as a client and try to
+ * connect to the server.
+ *
+ * The symlinks for all modules are created using the build system.
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <string>
+#include <algorithm>
+#include <fstream>
+#include <signal.h>
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <px4_time.h>
+#include <px4_log.h>
+#include <px4_getopt.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+
+#include "apps.h"
+#include "px4_middleware.h"
+#include "DriverFramework.hpp"
+#include "px4_middleware.h"
+#include "px4_daemon/client.h"
+#include "px4_daemon/server.h"
+#include "px4_daemon/pxh.h"
+
+#define MODULE_NAME "px4"
+
+static const char *LOCK_FILE_PATH = "/tmp/px4_lock";
+
+#ifndef PATH_MAX
+#define PATH_MAX 1024
+#endif
+
+
+static volatile bool _exit_requested = false;
+
+
+namespace px4
+{
+void init_once();
+}
+
+static void sig_int_handler(int sig_num);
+static void sig_fpe_handler(int sig_num);
+
+static void register_sig_handler();
+static void set_cpu_scaling();
+static int create_symlinks_if_needed(std::string &data_path);
+static int create_dirs();
+static int run_startup_script(const std::string &commands_file, const std::string &absolute_binary_path, int instance);
+static std::string get_absolute_binary_path(const std::string &argv0);
+static void wait_to_exit();
+static bool is_already_running(int instance);
+static void print_usage();
+static bool dir_exists(const std::string &path);
+static bool file_exists(const std::string &name);
+static std::string file_basename(std::string const &pathname);
+static std::string pwd();
+static int change_directory(const std::string &directory);
+
+
+#ifdef __PX4_SITL_MAIN_OVERRIDE
+int SITL_MAIN(int argc, char **argv);
+
+int SITL_MAIN(int argc, char **argv)
+#else
+int main(int argc, char **argv)
+#endif
+{
+	bool is_client = false;
+	bool pxh_off = false;
+
+	/* Symlinks point to all commands that can be used as a client with a prefix. */
+	const char prefix[] = PX4_SHELL_COMMAND_PREFIX;
+	int path_length = 0;
+
+	std::string absolute_binary_path; // full path to the px4 binary being executed
+
+	if (argc > 0) {
+		/* The executed binary name could start with a path, so strip it away */
+		const std::string full_binary_name = argv[0];
+		const std::string binary_name = file_basename(full_binary_name);
+
+		if (binary_name.compare(0, strlen(prefix), prefix) == 0) {
+			is_client = true;
+		}
+
+		path_length = full_binary_name.length() - binary_name.length();
+
+		absolute_binary_path = get_absolute_binary_path(full_binary_name);
+	}
+
+
+	if (is_client) {
+		int instance = 0;
+
+		if (argc >= 3 && strcmp(argv[1], "--instance") == 0) {
+			instance = strtoul(argv[2], nullptr, 10);
+			/* update argv so that "--instance <instance>" is not visible anymore */
+			argc -= 2;
+
+			for (int i = 1; i < argc; ++i) {
+				argv[i] = argv[i + 2];
+			}
+		}
+
+		PX4_DEBUG("instance: %i", instance);
+
+		if (!is_already_running(instance)) {
+			if (errno) {
+				PX4_ERR("Failed to communicate with daemon: %s", strerror(errno));
+
+			} else {
+				PX4_ERR("PX4 daemon not running yet");
+			}
+
+			return -1;
+		}
+
+		/* Remove the path and prefix. */
+		argv[0] += path_length + strlen(prefix);
+
+		px4_daemon::Client client(instance);
+		return client.process_args(argc, (const char **)argv);
+
+	} else {
+		/* Server/daemon apps need to parse the command line arguments. */
+
+		std::string data_path;
+		std::string commands_file = "etc/init.d/rcS";
+		std::string test_data_path;
+		std::string working_directory;
+		int instance = 0;
+
+		int myoptind = 1;
+		int ch;
+		const char *myoptarg = nullptr;
+
+		while ((ch = px4_getopt(argc, argv, "hdt:s:i:w:", &myoptind, &myoptarg)) != EOF) {
+			switch (ch) {
+			case 'h':
+				print_usage();
+				return 0;
+
+			case 'd':
+				pxh_off = true;
+				break;
+
+			case 't':
+				test_data_path = myoptarg;
+				break;
+
+			case 's':
+				commands_file = myoptarg;
+				break;
+
+			case 'i':
+				instance = strtoul(myoptarg, nullptr, 10);
+				break;
+
+			case 'w':
+				working_directory = myoptarg;
+				break;
+
+			default:
+				PX4_ERR("unrecognized flag");
+				print_usage();
+				return -1;
+			}
+		}
+
+		PX4_DEBUG("instance: %i", instance);
+
+		// change the CWD befre setting up links and other directories
+		if (!working_directory.empty()) {
+			int ret = change_directory(working_directory);
+
+			if (ret != PX4_OK) {
+				return ret;
+			}
+		}
+
+		if (myoptind < argc) {
+			std::string optional_arg = argv[myoptind];
+
+			if (optional_arg.compare(0, 2, "__") != 0 || optional_arg.find(":=") == std::string::npos) {
+				data_path = optional_arg;
+			} // else: ROS argument (in the form __<name>:=<value>)
+		}
+
+		if (is_already_running(instance)) {
+			// allow running multiple instances, but the server is only started for the first
+			PX4_INFO("PX4 daemon already running for instance %i (%s)", instance, strerror(errno));
+			return -1;
+		}
+
+		int ret = create_symlinks_if_needed(data_path);
+
+		if (ret != PX4_OK) {
+			return ret;
+		}
+
+		if (test_data_path != "") {
+			const std::string required_test_data_path = "./test_data";
+
+			if (!dir_exists(required_test_data_path)) {
+				ret = symlink(test_data_path.c_str(), required_test_data_path.c_str());
+
+				if (ret != PX4_OK) {
+					return ret;
+				}
+			}
+		}
+
+		if (!file_exists(commands_file)) {
+			PX4_ERR("Error opening startup file, does not exist: %s", commands_file.c_str());
+			return -1;
+		}
+
+		register_sig_handler();
+		set_cpu_scaling();
+
+		px4_daemon::Server server(instance);
+		server.start();
+
+		ret = create_dirs();
+
+		if (ret != PX4_OK) {
+			return ret;
+		}
+
+		DriverFramework::Framework::initialize();
+
+		px4::init_once();
+		px4::init(argc, argv, "px4");
+
+		ret = run_startup_script(commands_file, absolute_binary_path, instance);
+
+		// We now block here until we need to exit.
+		if (pxh_off) {
+			wait_to_exit();
+
+		} else {
+			px4_daemon::Pxh pxh;
+			pxh.run_pxh();
+		}
+
+		// When we exit, we need to stop muorb on Snapdragon.
+
+#ifdef __PX4_POSIX_EAGLE
+		// Sending muorb stop is needed if it is running to exit cleanly.
+		// TODO: we should check with px4_task_is_running("muorb") before stopping it.
+		std::string muorb_stop_cmd("muorb stop");
+		px4_daemon::Pxh::process_line(muorb_stop_cmd, true);
+#endif
+
+		std::string cmd("shutdown");
+		px4_daemon::Pxh::process_line(cmd, true);
+
+	}
+
+	return PX4_OK;
+}
+
+int create_symlinks_if_needed(std::string &data_path)
+{
+	std::string current_path = pwd();
+
+	if (data_path.empty()) {
+		// No data path given, we'll just try to use the current working dir.
+		data_path = current_path;
+		PX4_INFO("assuming working directory is rootfs, no symlinks needed.");
+		return PX4_OK;
+	}
+
+	if (data_path == current_path) {
+		// We are already running in the data path, so no need to symlink
+		PX4_INFO("working directory seems to be rootfs, no symlinks needed");
+		return PX4_OK;
+	}
+
+	const std::string path_sym_link = "etc";
+
+	PX4_DEBUG("path sym link: %s", path_sym_link.c_str());
+
+	std::string src_path = data_path;
+	std::string dest_path = current_path + "/" + path_sym_link;
+
+	struct stat info;
+
+	if (lstat(dest_path.c_str(), &info) == 0) {
+		if (S_ISLNK(info.st_mode)) {
+			// recreate the symlink, as it might point to some other location than what we want now
+			unlink(dest_path.c_str());
+
+		} else if (S_ISDIR(info.st_mode)) {
+			return PX4_OK;
+		}
+
+	}
+
+	PX4_INFO("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str());
+
+	// create sym-link
+	int ret = symlink(src_path.c_str(), dest_path.c_str());
+
+	if (ret != 0) {
+		PX4_ERR("Error creating symlink %s -> %s", src_path.c_str(), dest_path.c_str());
+		return ret;
+
+	} else {
+		PX4_DEBUG("Successfully created symlink %s -> %s", src_path.c_str(), dest_path.c_str());
+	}
+
+	return PX4_OK;
+}
+
+int create_dirs()
+{
+	std::string current_path = pwd();
+
+	std::vector<std::string> dirs{"log", "eeprom"};
+
+	for (const auto &dir : dirs) {
+		PX4_DEBUG("mkdir: %s", dir.c_str());;
+		std::string dir_path = current_path + "/" + dir;
+
+		if (dir_exists(dir_path)) {
+			continue;
+		}
+
+		// create dirs
+		int ret = mkdir(dir_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
+
+		if (ret != OK) {
+			PX4_WARN("failed creating new dir: %s", dir_path.c_str());
+			return ret;
+
+		} else {
+			PX4_DEBUG("Successfully created dir %s", dir_path.c_str());
+		}
+	}
+
+	return PX4_OK;
+}
+
+void register_sig_handler()
+{
+	// SIGINT
+	struct sigaction sig_int {};
+	sig_int.sa_handler = sig_int_handler;
+	sig_int.sa_flags = 0;// not SA_RESTART!
+
+	// SIGFPE
+	struct sigaction sig_fpe {};
+	sig_fpe.sa_handler = sig_fpe_handler;
+	sig_fpe.sa_flags = 0;// not SA_RESTART!
+
+	// SIGPIPE
+	// We want to ignore if a PIPE has been closed.
+	struct sigaction sig_pipe {};
+	sig_pipe.sa_handler = SIG_IGN;
+
+#ifdef __PX4_CYGWIN
+	// Do not catch SIGINT on Cygwin such that the process gets killed
+	// TODO: All threads should exit gracefully see https://github.com/PX4/Firmware/issues/11027
+	(void)sig_int; // this variable is unused
+#else
+	sigaction(SIGINT, &sig_int, nullptr);
+#endif
+
+	//sigaction(SIGTERM, &sig_int, nullptr);
+	sigaction(SIGFPE, &sig_fpe, nullptr);
+	sigaction(SIGPIPE, &sig_pipe, nullptr);
+}
+
+void sig_int_handler(int sig_num)
+{
+	fflush(stdout);
+	printf("\nExiting...\n");
+	fflush(stdout);
+	px4_daemon::Pxh::stop();
+	_exit_requested = true;
+}
+
+void sig_fpe_handler(int sig_num)
+{
+	fflush(stdout);
+	printf("\nfloating point exception\n");
+	fflush(stdout);
+	px4_daemon::Pxh::stop();
+	_exit_requested = true;
+}
+
+void set_cpu_scaling()
+{
+#ifdef __PX4_POSIX_EAGLE
+	// On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run
+	// at the maximum frequency all the time.
+	// Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon.
+	system("echo performance > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor");
+	system("echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor");
+
+	// Alternatively we could also raise the minimum frequency to save some power,
+	// unfortunately this still lead to some drops.
+	//system("echo 1190400 > /sys/devices/system/cpu/cpu0/cpufreq/scaling_min_freq");
+#endif
+}
+
+std::string get_absolute_binary_path(const std::string &argv0)
+{
+	// On Linux we could also use readlink("/proc/self/exe", buf, bufsize) to get the absolute path
+
+	std::size_t last_slash = argv0.find_last_of('/');
+
+	if (last_slash == std::string::npos) {
+		// either relative path or in PATH (PATH is ignored here)
+		return pwd();
+	}
+
+	std::string base = argv0.substr(0, last_slash);
+
+	if (base.length() > 0 && base[0] == '/') {
+		// absolute path
+		return base;
+	}
+
+	// relative path
+	return pwd() + "/" + base;
+}
+
+int run_startup_script(const std::string &commands_file, const std::string &absolute_binary_path,
+		       int instance)
+{
+	std::string shell_command("/bin/sh ");
+
+	shell_command += commands_file + ' ' + std::to_string(instance);
+
+	// Update the PATH variable to include the absolute_binary_path
+	// (required for the px4-alias.sh script and px4-* commands).
+	// They must be within the same directory as the px4 binary
+	const char *path_variable = "PATH";
+	std::string updated_path = absolute_binary_path;
+	const char *path = getenv(path_variable);
+
+	if (path) {
+		std::string spath = path;
+
+		// Check if absolute_binary_path already in PATH
+		bool already_in_path = false;
+		std::size_t current, previous = 0;
+		current = spath.find(':');
+
+		while (current != std::string::npos) {
+			if (spath.substr(previous, current - previous) == absolute_binary_path) {
+				already_in_path = true;
+			}
+
+			previous = current + 1;
+			current = spath.find(':', previous);
+		}
+
+		if (spath.substr(previous, current - previous) == absolute_binary_path) {
+			already_in_path = true;
+		}
+
+		if (!already_in_path) {
+			// Prepend to path to prioritize PX4 commands over potentially already installed PX4 commands.
+			updated_path = updated_path + ":" + path;
+			setenv(path_variable, updated_path.c_str(), 1);
+		}
+	}
+
+
+	PX4_INFO("Calling startup script: %s", shell_command.c_str());
+
+	int ret = 0;
+
+	if (!shell_command.empty()) {
+		ret = system(shell_command.c_str());
+
+		if (ret == 0) {
+			PX4_INFO("Startup script returned successfully");
+
+		} else {
+			PX4_ERR("Startup script returned with return value: %d", ret);
+		}
+
+	} else {
+		PX4_INFO("Startup script empty");
+	}
+
+	return ret;
+}
+
+void wait_to_exit()
+{
+	while (!_exit_requested) {
+		px4_usleep(100000);
+	}
+}
+
+void print_usage()
+{
+	printf("Usage for Server/daemon process: \n");
+	printf("\n");
+	printf("    px4 [-h|-d] [-s <startup_file>] [-t <test_data_directory>] [<rootfs_directory>] [-i <instance>] [-w <working_directory>]\n");
+	printf("\n");
+	printf("    -s <startup_file>      shell script to be used as startup (default=etc/init.d/rcS)\n");
+	printf("    <rootfs_directory>     directory where startup files and mixers are located,\n");
+	printf("                           (if not given, CWD is used)\n");
+	printf("    -i <instance>          px4 instance id to run multiple instances [0...N], default=0\n");
+	printf("    -w <working_directory> directory to change to\n");
+	printf("    -h                     help/usage information\n");
+	printf("    -d                     daemon mode, don't start pxh shell\n");
+	printf("\n");
+	printf("Usage for client: \n");
+	printf("\n");
+	printf("    px4-MODULE [--instance <instance>] command using symlink.\n");
+	printf("        e.g.: px4-commander status\n");
+}
+
+bool is_already_running(int instance)
+{
+	const std::string file_lock_path = std::string(LOCK_FILE_PATH) + '-' + std::to_string(instance);
+	struct flock fl;
+	int fd = open(file_lock_path.c_str(), O_RDWR | O_CREAT, 0666);
+
+	if (fd < 0) {
+		return false;
+	}
+
+	fl.l_type   = F_WRLCK;
+	fl.l_whence = SEEK_SET;
+	fl.l_start  = 0;
+	fl.l_len    = 0;
+	fl.l_pid    = getpid();
+
+	if (fcntl(fd, F_SETLK, &fl) == -1) {
+		// We failed to create a file lock, must be already locked.
+		if (errno == EACCES || errno == EAGAIN) {
+			return true;
+		}
+
+		return false;
+	}
+
+	errno = 0;
+	return false;
+}
+
+bool file_exists(const std::string &name)
+{
+	struct stat buffer;
+	return (stat(name.c_str(), &buffer) == 0);
+}
+
+static std::string file_basename(std::string const &pathname)
+{
+	struct MatchPathSeparator {
+		bool operator()(char ch) const
+		{
+			return ch == '/';
+		}
+	};
+	return std::string(std::find_if(pathname.rbegin(), pathname.rend(),
+					MatchPathSeparator()).base(), pathname.end());
+}
+
+bool dir_exists(const std::string &path)
+{
+	struct stat info;
+
+	if (stat(path.c_str(), &info) != 0) {
+		return false;
+
+	} else if (info.st_mode & S_IFDIR) {
+		return true;
+
+	}
+
+	return false;
+}
+
+std::string pwd()
+{
+	char temp[PATH_MAX];
+	return (getcwd(temp, PATH_MAX) ? std::string(temp) : std::string(""));
+}
+
+int change_directory(const std::string &directory)
+{
+	// create directory
+	if (!dir_exists(directory)) {
+		int ret = mkdir(directory.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
+
+		if (ret == -1) {
+			PX4_ERR("Error creating directory: %s (%s)", directory.c_str(), strerror(errno));
+			return -1;
+		}
+	}
+
+	// change directory
+	int ret = chdir(directory.c_str());
+
+	if (ret == -1) {
+		PX4_ERR("Error changing current path to: %s (%s)", directory.c_str(), strerror(errno));
+		return -1;
+	}
+
+	return PX4_OK;
+}

platforms/posix/src/px4/common/px4-alias.sh_in

@@ -0,0 +1,39 @@
+#!/bin/sh
+
+# File is auto-generated by cmake compilation, do not edit.
+
+# Map the NuttX-style variable definition 'set <var> <value>' to something that
+# bash and alternatives understand
+# define _set first because sh (POSIX shell) does not like overwriting set directly
+_set() {
+	eval $1=$2
+}
+alias set=_set
+# alternative method with an alias:
+# alias set='f(){ set -- "$1=$2"; eval "$1"; unset -f f; }; eval f'
+
+# Execute another shell script.
+# $1: Path to the script, (optionally starts with /, to match with the NuttX
+# scripts)
+sh() {
+	script="$1"
+	case "$script" in
+	"/"*)
+		script="$script"
+		;;
+	*)
+		script="/$script"
+		;;
+	esac
+	. "$(pwd)$script"
+}
+
+# Don't stop on errors.
+#set -e
+
+# Arguments passed to this script:
+# $1: optional instance id
+px4_instance=0
+[ -n "$1" ] && px4_instance=$1
+
+${alias_string}

platforms/posix/src/px4/common/px4_daemon/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# 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 PX4 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.
+#
+############################################################################
+
+px4_add_library(px4_daemon
+		pxh.cpp
+		history.cpp
+		client.cpp
+		server.cpp
+		server_io.cpp
+		sock_protocol.cpp
+	)
+

platforms/posix/src/px4/common/px4_daemon/client.cpp

@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file client.cpp
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ * @author Mara Bos <m-ou.se@m-ou.se>
+ */
+
+#include <errno.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <sys/un.h>
+#include <unistd.h>
+
+#include <string>
+
+#include <px4_log.h>
+#include "client.h"
+
+namespace px4_daemon
+{
+
+Client::Client(int instance_id) :
+	_fd(-1),
+	_instance_id(instance_id)
+{}
+
+int
+Client::process_args(const int argc, const char **argv)
+{
+	std::string sock_path = get_socket_path(_instance_id);
+
+	_fd = socket(AF_UNIX, SOCK_STREAM, 0);
+
+	if (_fd < 0) {
+		PX4_ERR("error creating socket");
+		return -1;
+	}
+
+	sockaddr_un addr = {};
+	addr.sun_family = AF_UNIX;
+	strncpy(addr.sun_path, sock_path.c_str(), sizeof(addr.sun_path) - 1);
+
+	if (connect(_fd, (sockaddr *)&addr, sizeof(addr)) < 0) {
+		PX4_ERR("error connecting to socket");
+		return -1;
+	}
+
+	int ret = _send_cmds(argc, argv);
+
+	if (ret != 0) {
+		PX4_ERR("Could not send commands");
+		return -3;
+	}
+
+	return _listen();
+}
+
+int
+Client::_send_cmds(const int argc, const char **argv)
+{
+	std::string cmd_buf;
+
+	for (int i = 0; i < argc; ++i) {
+		cmd_buf += argv[i];
+
+		if (i + 1 != argc) {
+			// TODO: Use '\0' as argument separator (and parse this server-side as well),
+			// so (quoted) whitespace within arguments doesn't get lost.
+			cmd_buf += " ";
+		}
+	}
+
+	// Last byte is 'isatty'.
+	cmd_buf.push_back(isatty(STDOUT_FILENO));
+
+	size_t n = cmd_buf.size();
+	const char *buf = cmd_buf.data();
+
+	while (n > 0) {
+		int n_sent = write(_fd, buf, n);
+
+		if (n_sent < 0) {
+			PX4_ERR("write() failed: %s", strerror(errno));
+			return -1;
+		}
+
+		n -= n_sent;
+		buf += n_sent;
+	}
+
+	return 0;
+}
+
+int
+Client::_listen()
+{
+	char buffer[1024];
+	int n_buffer_used = 0;
+
+	// The response ends in {0, retval}. So when we detect a 0, or a 0 followed
+	// by another byte, we don't output it yet, until we know whether it was
+	// the end of the stream or not.
+	while (true) {
+		int n_read = read(_fd, buffer + n_buffer_used, sizeof buffer - n_buffer_used);
+
+		if (n_read < 0) {
+			PX4_ERR("unable to read from socket");
+			return -1;
+
+		} else if (n_read == 0) {
+			if (n_buffer_used == 2) {
+				return buffer[1];
+
+			} else {
+				// Missing return value at end of stream. Stream was abruptly ended.
+				return -1;
+			}
+
+		} else {
+			n_read += n_buffer_used;
+
+			if (n_read >= 2 && buffer[n_read - 2] == 0) {
+				// If the buffer ends in {0, retval}, keep it.
+				fwrite(buffer, n_read - 2, 1, stdout);
+				buffer[0] = 0;
+				buffer[1] = buffer[n_read - 1];
+				n_buffer_used = 2;
+
+			} else if (n_read >= 1 && buffer[n_read - 1] == 0) {
+				// If the buffer ends in a 0-byte, keep it.
+				fwrite(buffer, n_read - 1, 1, stdout);
+				buffer[0] = 0;
+				n_buffer_used = 1;
+
+			} else {
+				fwrite(buffer, n_read, 1, stdout);
+				n_buffer_used = 0;
+			}
+		}
+	}
+}
+
+Client::~Client()
+{
+	if (_fd >= 0) {
+		close(_fd);
+	}
+}
+
+} // namespace px4_daemon

platforms/posix/src/px4/common/px4_daemon/client.h

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file client.h
+ *
+ * The client can connect and write a command to the socket that is supplied by
+ * the server. It will then close its half of the connection, and read back the
+ * stdout stream of the process that it started, followed by its return value.
+ *
+ * It the client dies, the connection gets closed automatically and the corresponding
+ * thread in the server gets cancelled.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ * @author Mara Bos <m-ou.se@m-ou.se>
+ */
+#pragma once
+
+#include <stdint.h>
+
+#include "sock_protocol.h"
+
+namespace px4_daemon
+{
+
+
+class Client
+{
+public:
+	Client(int instance_id = 0);
+
+	~Client();
+
+	Client(Client &&other) : _fd(other._fd), _instance_id(other._instance_id)
+	{
+		// Steal the fd from the moved-from client.
+		other._fd = -1;
+	}
+
+	/**
+	 * Process the supplied command line arguments and send them to server.
+	 *
+	 * @param argc: number of arguments
+	 * @param argv: argument values
+	 * @return 0 on success
+	 */
+	int process_args(const int argc, const char **argv);
+
+private:
+	int _send_cmds(const int argc, const char **argv);
+	int _listen();
+
+	int _fd;
+	int _instance_id; ///< instance ID for running multiple instances of the px4 server
+};
+
+} // namespace px4_daemon
+

platforms/posix/src/px4/common/px4_daemon/history.cpp

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015-2016 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file history.cpp
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string>
+#include <vector>
+
+#include "history.h"
+
+namespace px4_daemon
+{
+
+void History::try_to_add(const std::string &line)
+{
+	// Don't save an empty line.
+	if (line.empty()) {
+		return;
+	}
+
+	// Don't add duplicate entries.
+	if (!_history.empty() && line == _history.back()) {
+		return;
+	}
+
+	if (_history.size() == MAX_HISTORY_SIZE) {
+		_history.erase(_history.begin());
+	}
+
+	_history.push_back(line);
+}
+
+void History::reset_to_end()
+{
+	_current_history_entry = _history.end();
+}
+
+void History::try_to_save_current_line(const std::string &line)
+{
+	// Don't save what's currently entered line if there is no history
+	// entry to switch to.
+	if (_history.empty()) {
+		return;
+	}
+
+	// Don't save the current line if we are already jumping around in
+	// the history, and we must have already saved it.
+	if (_current_history_entry != _history.end()) {
+		return;
+	}
+
+	_current_line = line;
+}
+
+void History::get_previous(std::string &line)
+{
+	if (_history.empty()) {
+		return;
+	}
+
+	if (_current_history_entry == _history.begin()) {
+		return;
+	}
+
+	_current_history_entry = std::prev(_current_history_entry);
+	line = *_current_history_entry;
+}
+
+void History::get_next(std::string &line)
+{
+	if (_history.empty()) {
+		return;
+	}
+
+	// Already at the end, don't even try to get the next.
+	if (_current_history_entry == _history.end()) {
+		line = _current_line;
+		return;
+	}
+
+	_current_history_entry = std::next(_current_history_entry);
+
+	// We might have reached next now, ignore it and use what we saved
+	// in the beginning.
+	if (_current_history_entry == _history.end()) {
+		line = _current_line;
+		return;
+	}
+
+	line = *_current_history_entry;
+}
+
+} // namespace px4_daemon

platforms/posix/src/px4/common/px4_daemon/history.h

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file history.h
+ *
+ * Simple command history for the PX4 shell (pxh).
+ *
+ * This allows to go back in the history of entered commands.
+ * Additionally, before going back in the history, the current prompt can get saved.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+#pragma once
+
+#include <vector>
+#include <string>
+
+namespace px4_daemon
+{
+
+class History
+{
+public:
+	/**
+	 * Try to append the current line to the history.
+	 * Ignore the line if it is empty or duplicate of the
+	 * last added one.
+	 *
+	 * Drop the first entry of the history if we reach the
+	 * MAX_HISTORY_SIZE.
+	 *
+	 * @param line: command line to be added.
+	 */
+	void try_to_add(const std::string &line);
+
+	/**
+	 * After executing a command in the shell, we want to be at
+	 * the end of the history again.
+	 */
+	void reset_to_end();
+
+	/**
+	 * If we start scrolling up in the history, we can try to save
+	 * the current command line. When we scroll back down, we can
+	 * get it out again.
+	 *
+	 * @param line: line to be saved
+	 */
+	void try_to_save_current_line(const std::string &line);
+
+
+	/**
+	 * Set the previous (earlier) command from the history.
+	 *
+	 * @param line: swap to previous line if available.
+	 */
+	void get_previous(std::string &line);
+
+	/**
+	 * Set the next (more recent) command from the history.
+	 *
+	 * @param line: swap to next line if available, otherwise saved current.
+	 */
+	void get_next(std::string &line);
+
+	static const unsigned MAX_HISTORY_SIZE = 100;
+private:
+	std::vector<std::string> _history;
+	std::vector<std::string>::iterator _current_history_entry;
+	std::string _current_line;
+};
+
+} // namespace px4_daemon
+

platforms/posix/src/px4/common/px4_daemon/pxh.cpp

@@ -0,0 +1,295 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015-2016 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file pxh.cpp
+ *
+ * This is a simple PX4 shell implementation used to start modules.
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string>
+#include <sstream>
+#include <vector>
+#include <stdio.h>
+
+#include "pxh.h"
+
+namespace px4_daemon
+{
+
+Pxh *Pxh::_instance = nullptr;
+
+apps_map_type Pxh::_apps = {};
+
+
+Pxh::Pxh()
+{
+	_instance = this;
+}
+
+Pxh::~Pxh()
+{
+	_instance = nullptr;
+}
+
+int Pxh::process_line(const std::string &line, bool silently_fail)
+{
+	if (line.empty()) {
+		return 0;
+	}
+
+	if (_apps.empty()) {
+		init_app_map(_apps);
+	}
+
+	std::stringstream line_stream(line);
+	std::string word;
+	std::vector<std::string> words;
+
+	// First arg should be the command.
+	while (line_stream >> word) {
+		words.push_back(word);
+	}
+
+	if (words.empty()) {
+		return 0;
+	}
+
+	const std::string &command(words.front());
+
+	if (_apps.find(command) != _apps.end()) {
+
+		// Note that argv[argc] always needs to be a nullptr.
+		// Therefore add one more entry.
+		const char *arg[words.size() + 1];
+
+		for (unsigned i = 0; i < words.size(); ++i) {
+			arg[i] = (char *)words[i].c_str();
+		}
+
+		// Explicitly set this nullptr.
+		arg[words.size()] = nullptr;
+
+		int retval = _apps[command](words.size(), (char **)arg);
+
+		if (retval) {
+			if (!silently_fail) {
+				printf("Command '%s' failed, returned %d.\n", command.c_str(), retval);
+			}
+		}
+
+		return retval;
+
+	} else if (command == "help") {
+		list_builtins(_apps);
+		return 0;
+
+	} else if (command.length() == 0 || command[0] == '#') {
+		// Do nothing
+		return 0;
+
+	} else if (!silently_fail) {
+		//std::cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl;
+		printf("Invalid command: %s\ntype 'help' for a list of commands\n", command.c_str());
+		return -1;
+
+	} else {
+		return -1;
+	}
+}
+
+
+void Pxh::run_pxh()
+{
+	_should_exit = false;
+
+	_setup_term();
+
+	std::string mystr;
+	int cursor_position = 0; // position of the cursor from right to left
+	// (0: all the way to the right, mystr.length: all the way to the left)
+
+	_print_prompt();
+
+	while (!_should_exit) {
+
+		int c = getchar();
+		std::string add_string; // string to add at current cursor position
+		bool update_prompt = true;
+
+		switch (c) {
+		case EOF:
+			_should_exit = true;
+			break;
+
+		case 127:	// backslash
+			if ((int)mystr.length() - cursor_position > 0) {
+				mystr.erase(mystr.length() - cursor_position - 1, 1);
+			}
+
+			break;
+
+		case '\n':	// user hit enter
+			_history.try_to_add(mystr);
+			_history.reset_to_end();
+
+			printf("\n");
+			process_line(mystr, false);
+			// reset string and cursor position
+			mystr = "";
+			cursor_position = 0;
+
+			update_prompt = false;
+			_print_prompt();
+			break;
+
+		case '\033': {	// arrow keys
+				c = getchar();	// skip first one, does not have the info
+				c = getchar();
+
+				if (c == 'A') { // arrow up
+					_history.try_to_save_current_line(mystr);
+					_history.get_previous(mystr);
+					cursor_position = 0; // move cursor to end of line
+
+				} else if (c == 'B') { // arrow down
+					_history.get_next(mystr);
+					cursor_position = 0; // move cursor to end of line
+
+				} else if (c == 'C') { // arrow right
+					if (cursor_position > 0) {
+						cursor_position--;
+					}
+
+				} else if (c == 'D') { // arrow left
+					if (cursor_position < (int)mystr.length()) {
+						cursor_position++;
+					}
+
+				} else if (c == 'H') { // Home (go to the beginning of the command)
+					cursor_position = mystr.length();
+
+				} else if (c == '1') { // Home (go to the beginning of the command, Editing key)
+					(void)getchar(); // swallow '~'
+					cursor_position = mystr.length();
+
+				} else if (c == 'F') { // End (go to the end of the command)
+					cursor_position = 0;
+
+				} else if (c == '4') { // End (go to the end of the command, Editing key)
+					(void)getchar(); // swallow '~'
+					cursor_position = 0;
+				}
+
+				break;
+			}
+
+		default:	// any other input
+			if (c > 3) {
+				add_string += (char)c;
+
+			} else {
+				update_prompt = false;
+			}
+
+			break;
+		}
+
+
+		if (update_prompt) {
+			// reprint prompt with mystr
+			mystr.insert(mystr.length() - cursor_position, add_string);
+			_clear_line();
+			_print_prompt();
+			printf("%s", mystr.c_str());
+
+			// Move the cursor to its position
+			if (cursor_position > 0) {
+				_move_cursor(cursor_position);
+			}
+		}
+
+	}
+}
+
+void Pxh::stop()
+{
+	_restore_term();
+
+	if (_instance) {
+		_instance->_should_exit = true;
+	}
+}
+
+void Pxh::_setup_term()
+{
+	// Make sure we restore terminal at exit.
+	tcgetattr(0, &_orig_term);
+	atexit(Pxh::_restore_term);
+
+	// change input mode so that we can manage shell
+	struct termios term;
+	tcgetattr(0, &term);
+	term.c_lflag &= ~ICANON;
+	term.c_lflag &= ~ECHO;
+	tcsetattr(0, TCSANOW, &term);
+	setbuf(stdin, nullptr);
+}
+
+void Pxh::_restore_term()
+{
+	if (_instance) {
+		tcsetattr(0, TCSANOW, &_instance->_orig_term);
+	}
+}
+
+void Pxh::_print_prompt()
+{
+	fflush(stdout);
+	printf("pxh> ");
+	fflush(stdout);
+}
+
+void Pxh::_clear_line()
+{
+	printf("%c[2K%c", (char)27, (char)13);
+}
+void Pxh::_move_cursor(int position)
+{
+	printf("\033[%dD", position);
+}
+
+} // namespace px4_daemon

platforms/posix/src/px4/common/px4_daemon/pxh.h

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file pxh.h
+ *
+ * The POSIX PX4 implementation features a simple shell to start modules
+ * or use system commands.
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ */
+#pragma once
+
+#include <vector>
+#include <string>
+#include <termios.h>
+
+#include <platforms/posix/apps.h>
+#include "history.h"
+
+namespace px4_daemon
+{
+
+
+class Pxh
+{
+public:
+	Pxh();
+	~Pxh();
+
+	/**
+	 * Process and run one command line.
+	 *
+	 * @param silently_fail: don't make a fuss on failure
+	 * @return 0 if successful. */
+	static int process_line(const std::string &line, bool silently_fail);
+
+	/**
+	 * Run the pxh shell. This will only return if stop() is called.
+	 */
+	void run_pxh();
+
+	/**
+	 * Can be called to stop pxh.
+	 */
+	static void stop();
+
+private:
+	void _print_prompt();
+	void _move_cursor(int position);
+	void _clear_line();
+
+	void _setup_term();
+	static void _restore_term();
+
+	bool _should_exit;
+	History _history;
+	struct termios _orig_term;
+
+	static apps_map_type _apps;
+	static Pxh *_instance;
+};
+
+
+
+} // namespace px4_daemon
+

platforms/posix/src/px4/common/px4_daemon/server.cpp

@@ -0,0 +1,310 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file server.cpp
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ * @author Mara Bos <m-ou.se@m-ou.se>
+ */
+
+#include <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <string>
+#include <pthread.h>
+#include <poll.h>
+#include <sys/stat.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <sys/un.h>
+#include <vector>
+
+#include <px4_log.h>
+
+#include "pxh.h"
+#include "server.h"
+
+namespace px4_daemon
+{
+
+Server *Server::_instance = nullptr;
+
+Server::Server(int instance_id)
+	: _mutex(PTHREAD_MUTEX_INITIALIZER),
+	  _instance_id(instance_id)
+{
+	_instance = this;
+}
+
+Server::~Server()
+{
+	_instance = nullptr;
+}
+
+int
+Server::start()
+{
+	std::string sock_path = get_socket_path(_instance_id);
+
+	// Delete socket in case it exists already.
+	unlink(sock_path.c_str());
+
+	_fd = socket(AF_UNIX, SOCK_STREAM, 0);
+
+	if (_fd < 0) {
+		PX4_ERR("error creating socket");
+		return -1;
+	}
+
+	sockaddr_un addr = {};
+	addr.sun_family = AF_UNIX;
+	strncpy(addr.sun_path, sock_path.c_str(), sizeof(addr.sun_path) - 1);
+
+	if (bind(_fd, (sockaddr *)&addr, sizeof(addr)) < 0) {
+		PX4_ERR("error binding socket");
+		return -1;
+	}
+
+	if (listen(_fd, 10) < 0) {
+		PX4_ERR("error listing to socket");
+		return -1;
+	}
+
+	if (0 != pthread_create(&_server_main_pthread,
+				nullptr,
+				_server_main_trampoline,
+				this)) {
+		PX4_ERR("error creating client handler thread");
+		return -1;
+	}
+
+	return 0;
+}
+
+void *
+Server::_server_main_trampoline(void *self)
+{
+	((Server *)self)->_server_main();
+	return nullptr;
+}
+
+void Server::_pthread_key_destructor(void *arg)
+{
+	delete ((CmdThreadSpecificData *)arg);
+}
+
+void
+Server::_server_main()
+{
+	int ret = pthread_key_create(&_key, _pthread_key_destructor);
+
+	if (ret != 0) {
+		PX4_ERR("failed to create pthread key");
+		return;
+	}
+
+	// The list of file descriptors to watch.
+	std::vector<pollfd> poll_fds;
+
+	// Watch the listening socket for incoming connections.
+	poll_fds.push_back(pollfd {_fd, POLLIN, 0});
+
+	// The list of FILE pointers that we'll need to fclose().
+	// stdouts[i] corresponds to poll_fds[i+1].
+	std::vector<FILE *> stdouts;
+
+	while (true) {
+		int n_ready = poll(poll_fds.data(), poll_fds.size(), -1);
+
+		if (n_ready < 0) {
+			PX4_ERR("poll() failed: %s", strerror(errno));
+			return;
+		}
+
+		_lock();
+
+		// Handle any new connections.
+		if (poll_fds[0].revents & POLLIN) {
+			--n_ready;
+			int client = accept(_fd, nullptr, nullptr);
+
+			if (client == -1) {
+				PX4_ERR("failed to accept client: %s", strerror(errno));
+				_unlock();
+				return;
+			}
+
+			FILE *thread_stdout = fdopen(client, "w");
+
+			if (thread_stdout == nullptr) {
+				PX4_ERR("could not open stdout for new thread");
+				close(client);
+
+			} else {
+				// Set stream to line buffered.
+				setvbuf(thread_stdout, nullptr, _IOLBF, BUFSIZ);
+
+				// Start a new thread to handle the client.
+				pthread_t *thread = &_fd_to_thread[client];
+				ret = pthread_create(thread, nullptr, Server::_handle_client, thread_stdout);
+
+				if (ret != 0) {
+					PX4_ERR("could not start pthread (%i)", ret);
+					_fd_to_thread.erase(client);
+					fclose(thread_stdout);
+
+				} else {
+					// We won't join the thread, so detach to automatically release resources at its end
+					pthread_detach(*thread);
+
+					// Start listening for the client hanging up.
+					poll_fds.push_back(pollfd {client, POLLHUP, 0});
+
+					// Remember the FILE *, so we can fclose() it later.
+					stdouts.push_back(thread_stdout);
+				}
+			}
+		}
+
+		// Handle any closed connections.
+		for (size_t i = 1; n_ready > 0 && i < poll_fds.size();) {
+			if (poll_fds[i].revents) {
+				--n_ready;
+				auto thread = _fd_to_thread.find(poll_fds[i].fd);
+
+				if (thread != _fd_to_thread.end()) {
+					// Thread is still running, so we cancel it.
+					// TODO: use a more graceful exit method to avoid resource leaks
+					pthread_cancel(thread->second);
+					_fd_to_thread.erase(thread);
+				}
+
+				fclose(stdouts[i - 1]);
+				stdouts.erase(stdouts.begin() + i - 1);
+				poll_fds.erase(poll_fds.begin() + i);
+
+			} else {
+				++i;
+			}
+		}
+
+		_unlock();
+	}
+
+	close(_fd);
+}
+
+void
+*Server::_handle_client(void *arg)
+{
+	FILE *out = (FILE *)arg;
+	int fd = fileno(out);
+
+	// Read until the end of the incoming stream.
+	std::string cmd;
+
+	while (true) {
+		size_t n = cmd.size();
+		cmd.resize(n + 1024);
+		ssize_t n_read = read(fd, &cmd[n], cmd.size() - n);
+
+		if (n_read <= 0) {
+			_cleanup(fd);
+			return nullptr;
+		}
+
+		cmd.resize(n + n_read);
+
+		// Command ends in 0x00 (no tty) or 0x01 (tty).
+		if (!cmd.empty() && cmd.back() < 2) {
+			break;
+		}
+	}
+
+	if (cmd.size() < 2) {
+		_cleanup(fd);
+		return nullptr;
+	}
+
+	// Last byte is 'isatty'.
+	uint8_t isatty = cmd.back();
+	cmd.pop_back();
+
+	// We register thread specific data. This is used for PX4_INFO (etc.) log calls.
+	CmdThreadSpecificData *thread_data_ptr;
+
+	if ((thread_data_ptr = (CmdThreadSpecificData *)pthread_getspecific(_instance->_key)) == nullptr) {
+		thread_data_ptr = new CmdThreadSpecificData;
+		thread_data_ptr->thread_stdout = out;
+		thread_data_ptr->is_atty = isatty;
+
+		(void)pthread_setspecific(_instance->_key, (void *)thread_data_ptr);
+	}
+
+	// Run the actual command.
+	int retval = Pxh::process_line(cmd, true);
+
+	// Report return value.
+	char buf[2] = {0, (char)retval};
+
+	if (fwrite(buf, sizeof buf, 1, out) != 1) {
+		// Don't care it went wrong, as we're cleaning up anyway.
+	}
+
+	// Flush the FILE*'s buffer before we shut down the connection.
+	fflush(out);
+
+	_cleanup(fd);
+	return nullptr;
+}
+
+void
+Server::_cleanup(int fd)
+{
+	_instance->_lock();
+	_instance->_fd_to_thread.erase(fd);
+	_instance->_unlock();
+
+	// We can't close() the fd here, since the main thread is probably
+	// polling for it: close()ing it causes a race condition.
+	// So, we only call shutdown(), which causes the main thread to register a
+	// 'POLLHUP', such that the main thread can close() it for us.
+	// We already removed this thread from _fd_to_thread, so there is no risk
+	// of the main thread trying to cancel this thread after it already exited.
+	shutdown(fd, SHUT_RDWR);
+}
+
+} //namespace px4_daemon

platforms/posix/src/px4/common/px4_daemon/server.h

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file server.h
+ *
+ * The server (also called daemon) opens a socket for clients to connect to.
+ *
+ * Once a client connects it will send a command and close its side of the connection.
+ * The server will return the stdout of the executing command, as well as the return
+ * value to the client.
+ *
+ * There should only every be one server running, therefore the static instance.
+ * The Singleton implementation is not complete, but it should be obvious not
+ * to instantiate multiple servers.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ * @author Mara Bos <m-ou.se@m-ou.se>
+ */
+#pragma once
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <pthread.h>
+#include <map>
+
+#include "sock_protocol.h"
+
+
+namespace px4_daemon
+{
+
+class Server
+{
+public:
+	Server(int instance_id = 0);
+	~Server();
+
+	/**
+	 * Start the server. This will spawn a thread with a
+	 * while loop waiting for clients sending commands.
+	 *
+	 * @return 0 if started successfully
+	 */
+	int start();
+
+	struct CmdThreadSpecificData {
+		FILE *thread_stdout; // stdout of this thread
+		bool is_atty; // whether file descriptor refers to a terminal
+	};
+
+	static bool is_running()
+	{
+		return _instance != nullptr;
+	}
+
+	static pthread_key_t get_pthread_key()
+	{
+		return _instance->_key;
+	}
+private:
+	static void *_server_main_trampoline(void *arg);
+	void _server_main();
+
+	void _lock()
+	{
+		pthread_mutex_lock(&_mutex);
+	}
+
+	void _unlock()
+	{
+		pthread_mutex_unlock(&_mutex);
+	}
+
+	static void *_handle_client(void *arg);
+	static void _cleanup(int fd);
+
+	pthread_t _server_main_pthread;
+
+	std::map<int, pthread_t> _fd_to_thread;
+	pthread_mutex_t _mutex; ///< Protects _fd_to_thread.
+
+	pthread_key_t _key;
+
+	int _instance_id; ///< instance ID for running multiple instances of the px4 server
+
+	int _fd;
+
+	static void _pthread_key_destructor(void *arg);
+
+	static Server *_instance;
+};
+
+
+} // namespace px4_daemon
+

platforms/posix/src/px4/common/px4_daemon/server_io.cpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016-2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file server_io.cpp
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ * @author Mara Bos <m-ou.se@m-ou.se>
+ */
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <string>
+#include <pthread.h>
+#include <poll.h>
+#include <assert.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <px4_log.h>
+
+#include "server.h"
+#include <px4_daemon/server_io.h>
+#include "sock_protocol.h"
+
+
+using namespace px4_daemon;
+
+
+FILE *get_stdout(bool *isatty_)
+{
+	Server::CmdThreadSpecificData *thread_data_ptr;
+
+	// If we are not in a thread that has been started by a client, we don't
+	// have any thread specific data set and we won't have a pipe to write
+	// stdout to.
+	if (!Server::is_running() ||
+	    (thread_data_ptr = (Server::CmdThreadSpecificData *)pthread_getspecific(
+				       Server::get_pthread_key())) == nullptr) {
+		if (isatty_) { *isatty_ = isatty(1); }
+
+		return stdout;
+	}
+
+#ifdef __PX4_POSIX_EAGLE
+
+	// XXX FIXME: thread_data_ptr is set to 0x1 in the main thread on Snapdragon
+	// even though the pthread_key has been created.
+	// We can catch this using the check below but we have no clue why this happens.
+	if (thread_data_ptr == (void *)0x1) {
+		if (isatty_) { *isatty_ = isatty(1); }
+
+		return stdout;
+	}
+
+#endif
+
+	if (thread_data_ptr->thread_stdout == nullptr) {
+		if (isatty_) { *isatty_ = isatty(1); }
+
+		return stdout;
+	}
+
+	if (isatty_) { *isatty_ = thread_data_ptr->is_atty; }
+
+	return thread_data_ptr->thread_stdout;
+}

platforms/posix/src/px4/common/px4_daemon/sock_protocol.cpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file sock_protocol.cpp
+ *
+ * @author Mara Bos <m-ou.se@m-ou.se>
+ */
+
+#include "sock_protocol.h"
+
+namespace px4_daemon
+{
+
+std::string get_socket_path(int instance_id)
+{
+	// TODO: Use /var/run/px4/$instance/sock (or /var/run/user/$UID/... for non-root).
+	return "/tmp/px4-sock-" + std::to_string(instance_id);
+}
+
+} // namespace px4_daemon
+

platforms/posix/src/px4/common/px4_daemon/sock_protocol.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2018 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+/**
+ * @file sock_protocol.h
+ *
+ * @author Mara Bos <m-ou.se@m-ou.se>
+ */
+#pragma once
+
+#include <string>
+
+namespace px4_daemon
+{
+
+std::string get_socket_path(int instance_id);
+
+} // namespace px4_daemon
+

platforms/posix/src/px4/common/px4_init.cpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+#include "px4_init.h"
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <drivers/drv_hrt.h>
+#include <lib/parameters/param.h>
+#include <px4_work_queue/WorkQueueManager.hpp>
+
+int px4_platform_init(void)
+{
+	hrt_init();
+
+	param_init();
+
+	px4::WorkQueueManagerStart();
+
+	return PX4_OK;
+}

platforms/posix/src/px4/common/px4_posix_impl.cpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_posix_impl.cpp
+ *
+ * PX4 Middleware Wrapper Linux Implementation
+ */
+
+#include <px4_defines.h>
+#include <px4_middleware.h>
+#include <px4_workqueue.h>
+#include <px4_defines.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <signal.h>
+#include <errno.h>
+#include <unistd.h>
+#include <parameters/param.h>
+#include "hrt_work.h"
+#include <drivers/drv_hrt.h>
+#include "px4_time.h"
+#include <pthread.h>
+#include <px4_init.h>
+
+extern pthread_t _shell_task_id;
+
+__BEGIN_DECLS
+
+long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
+
+__END_DECLS
+
+namespace px4
+{
+
+void init_once();
+
+void init_once()
+{
+	_shell_task_id = pthread_self();
+	//printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id);
+
+	work_queues_init();
+	hrt_work_queue_init();
+
+	px4_platform_init();
+}
+
+void init(int argc, char *argv[], const char *app_name)
+{
+	printf("\n");
+	printf("______  __   __    ___ \n");
+	printf("| ___ \\ \\ \\ / /   /   |\n");
+	printf("| |_/ /  \\ V /   / /| |\n");
+	printf("|  __/   /   \\  / /_| |\n");
+	printf("| |     / /^\\ \\ \\___  |\n");
+	printf("\\_|     \\/   \\/     |_/\n");
+	printf("\n");
+	printf("%s starting.\n", app_name);
+	printf("\n");
+
+	// set the threads name
+#ifdef __PX4_DARWIN
+	(void)pthread_setname_np(app_name);
+#else
+	(void)pthread_setname_np(pthread_self(), app_name);
+#endif
+}
+
+uint64_t get_time_micros()
+{
+	return hrt_absolute_time();
+}
+
+}
+

platforms/posix/src/px4/common/px4_posix_tasks.cpp

@@ -0,0 +1,452 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   Author: @author Mark Charlebois <charlebm#gmail.com>
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_posix_tasks.c
+ * Implementation of existing task API for Linux
+ */
+
+#include <px4_log.h>
+#include <px4_defines.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <signal.h>
+#include <fcntl.h>
+#include <sched.h>
+#include <unistd.h>
+#include <string.h>
+#include <pthread.h>
+#include <limits.h>
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <string>
+
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <systemlib/err.h>
+
+#define MAX_CMD_LEN 100
+
+#define PX4_MAX_TASKS 50
+#define SHELL_TASK_ID (PX4_MAX_TASKS+1)
+
+pthread_t _shell_task_id = 0;
+pthread_mutex_t task_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+struct task_entry {
+	pthread_t pid;
+	std::string name;
+	bool isused;
+	task_entry() : isused(false) {}
+};
+
+static task_entry taskmap[PX4_MAX_TASKS] = {};
+
+typedef struct {
+	px4_main_t entry;
+	char name[16]; //pthread_setname_np is restricted to 16 chars
+	int argc;
+	char *argv[];
+	// strings are allocated after the struct data
+} pthdata_t;
+
+static void *entry_adapter(void *ptr)
+{
+	pthdata_t *data = (pthdata_t *) ptr;
+
+	int rv;
+
+	// set the threads name
+#ifdef __PX4_DARWIN
+	rv = pthread_setname_np(data->name);
+#else
+	rv = pthread_setname_np(pthread_self(), data->name);
+#endif
+
+	if (rv) {
+		PX4_ERR("px4_task_spawn_cmd: failed to set name of thread %d %d\n", rv, errno);
+	}
+
+	data->entry(data->argc, data->argv);
+	free(ptr);
+	PX4_DEBUG("Before px4_task_exit");
+	px4_task_exit(0);
+	PX4_DEBUG("After px4_task_exit");
+
+	return nullptr;
+}
+
+void
+px4_systemreset(bool to_bootloader)
+{
+	PX4_WARN("Called px4_system_reset");
+	system_exit(0);
+}
+
+px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
+			      char *const argv[])
+{
+
+	int i;
+	int argc = 0;
+	unsigned int len = 0;
+	struct sched_param param = {};
+	char *p = (char *)argv;
+
+	// Calculate argc
+	while (p != (char *)nullptr) {
+		p = argv[argc];
+
+		if (p == (char *)nullptr) {
+			break;
+		}
+
+		++argc;
+		len += strlen(p) + 1;
+	}
+
+	unsigned long structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *);
+
+	// not safe to pass stack data to the thread creation
+	pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len);
+
+	if (taskdata == nullptr) {
+		return -ENOMEM;
+	}
+
+	memset(taskdata, 0, structsize + len);
+	unsigned long offset = ((unsigned long)taskdata) + structsize;
+
+	strncpy(taskdata->name, name, 16);
+	taskdata->name[15] = 0;
+	taskdata->entry = entry;
+	taskdata->argc = argc;
+
+	for (i = 0; i < argc; i++) {
+		PX4_DEBUG("arg %d %s\n", i, argv[i]);
+		taskdata->argv[i] = (char *)offset;
+		strcpy((char *)offset, argv[i]);
+		offset += strlen(argv[i]) + 1;
+	}
+
+	// Must add NULL at end of argv
+	taskdata->argv[argc] = (char *)nullptr;
+
+	PX4_DEBUG("starting task %s", name);
+
+	pthread_attr_t attr;
+	int rv = pthread_attr_init(&attr);
+
+	if (rv != 0) {
+		PX4_ERR("px4_task_spawn_cmd: failed to init thread attrs");
+		free(taskdata);
+		return (rv < 0) ? rv : -rv;
+	}
+
+#ifndef __PX4_DARWIN
+
+	if (stack_size < PTHREAD_STACK_MIN) {
+		stack_size = PTHREAD_STACK_MIN;
+	}
+
+	rv = pthread_attr_setstacksize(&attr, PX4_STACK_ADJUSTED(stack_size));
+
+	if (rv != 0) {
+		PX4_ERR("pthread_attr_setstacksize to %d returned error (%d)", stack_size, rv);
+		pthread_attr_destroy(&attr);
+		free(taskdata);
+		return (rv < 0) ? rv : -rv;
+	}
+
+#endif
+
+	rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
+
+	if (rv != 0) {
+		PX4_ERR("px4_task_spawn_cmd: failed to set inherit sched");
+		pthread_attr_destroy(&attr);
+		free(taskdata);
+		return (rv < 0) ? rv : -rv;
+	}
+
+	rv = pthread_attr_setschedpolicy(&attr, scheduler);
+
+	if (rv != 0) {
+		PX4_ERR("px4_task_spawn_cmd: failed to set sched policy");
+		pthread_attr_destroy(&attr);
+		free(taskdata);
+		return (rv < 0) ? rv : -rv;
+	}
+
+#ifdef __PX4_CYGWIN
+	/* Priorities on Windows are defined a lot differently */
+	priority = SCHED_PRIORITY_DEFAULT;
+#endif
+
+	param.sched_priority = priority;
+
+	rv = pthread_attr_setschedparam(&attr, &param);
+
+	if (rv != 0) {
+		PX4_ERR("px4_task_spawn_cmd: failed to set sched param");
+		pthread_attr_destroy(&attr);
+		free(taskdata);
+		return (rv < 0) ? rv : -rv;
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	px4_task_t taskid = 0;
+
+	for (i = 0; i < PX4_MAX_TASKS; ++i) {
+		if (!taskmap[i].isused) {
+			taskmap[i].name = name;
+			taskmap[i].isused = true;
+			taskid = i;
+			break;
+		}
+	}
+
+	if (i >= PX4_MAX_TASKS) {
+		pthread_attr_destroy(&attr);
+		pthread_mutex_unlock(&task_mutex);
+		free(taskdata);
+		return -ENOSPC;
+	}
+
+	rv = pthread_create(&taskmap[taskid].pid, &attr, &entry_adapter, (void *) taskdata);
+
+	if (rv != 0) {
+
+		if (rv == EPERM) {
+			//printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
+			rv = pthread_create(&taskmap[taskid].pid, nullptr, &entry_adapter, (void *) taskdata);
+
+			if (rv != 0) {
+				PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
+				taskmap[taskid].isused = false;
+				pthread_attr_destroy(&attr);
+				pthread_mutex_unlock(&task_mutex);
+				free(taskdata);
+				return (rv < 0) ? rv : -rv;
+			}
+
+		} else {
+			pthread_attr_destroy(&attr);
+			pthread_mutex_unlock(&task_mutex);
+			free(taskdata);
+			return (rv < 0) ? rv : -rv;
+		}
+	}
+
+	pthread_attr_destroy(&attr);
+	pthread_mutex_unlock(&task_mutex);
+
+	return taskid;
+}
+
+int px4_task_delete(px4_task_t id)
+{
+	int rv = 0;
+	pthread_t pid;
+	PX4_DEBUG("Called px4_task_delete");
+
+	if (id < PX4_MAX_TASKS && taskmap[id].isused) {
+		pid = taskmap[id].pid;
+
+	} else {
+		return -EINVAL;
+	}
+
+	pthread_mutex_lock(&task_mutex);
+
+	// If current thread then exit, otherwise cancel
+	if (pthread_self() == pid) {
+		pthread_join(pid, nullptr);
+		taskmap[id].isused = false;
+		pthread_mutex_unlock(&task_mutex);
+		pthread_exit(nullptr);
+
+	} else {
+		rv = pthread_cancel(pid);
+	}
+
+	taskmap[id].isused = false;
+	pthread_mutex_unlock(&task_mutex);
+
+	return rv;
+}
+
+void px4_task_exit(int ret)
+{
+	int i;
+	pthread_t pid = pthread_self();
+
+	// Get pthread ID from the opaque ID
+	for (i = 0; i < PX4_MAX_TASKS; ++i) {
+		if (taskmap[i].pid == pid) {
+			pthread_mutex_lock(&task_mutex);
+			taskmap[i].isused = false;
+			break;
+		}
+	}
+
+	if (i >= PX4_MAX_TASKS)  {
+		PX4_ERR("px4_task_exit: self task not found!");
+
+	} else {
+		PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str());
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
+int px4_task_kill(px4_task_t id, int sig)
+{
+	int rv = 0;
+	pthread_t pid;
+	PX4_DEBUG("Called px4_task_kill %d", sig);
+
+	if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) {
+		pthread_mutex_lock(&task_mutex);
+		pid = taskmap[id].pid;
+		pthread_mutex_unlock(&task_mutex);
+
+	} else {
+		return -EINVAL;
+	}
+
+	// If current thread then exit, otherwise cancel
+	rv = pthread_kill(pid, sig);
+
+	return rv;
+}
+
+void px4_show_tasks()
+{
+	int idx;
+	int count = 0;
+
+	PX4_INFO("Active Tasks:");
+
+	for (idx = 0; idx < PX4_MAX_TASKS; idx++) {
+		if (taskmap[idx].isused) {
+			PX4_INFO("   %-10s %lu", taskmap[idx].name.c_str(), (unsigned long)taskmap[idx].pid);
+			count++;
+		}
+	}
+
+	if (count == 0) {
+		PX4_INFO("   No running tasks");
+	}
+
+}
+
+bool px4_task_is_running(const char *taskname)
+{
+	int idx;
+
+	for (idx = 0; idx < PX4_MAX_TASKS; idx++) {
+		if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+px4_task_t px4_getpid()
+{
+	pthread_t pid = pthread_self();
+	px4_task_t ret = -1;
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].pid == pid) {
+			ret = i;
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+	return ret;
+}
+
+const char *px4_get_taskname()
+{
+	pthread_t pid = pthread_self();
+	const char *prog_name = "UnknownApp";
+
+	pthread_mutex_lock(&task_mutex);
+
+	for (int i = 0; i < PX4_MAX_TASKS; i++) {
+		if (taskmap[i].isused && taskmap[i].pid == pid) {
+			prog_name = taskmap[i].name.c_str();
+		}
+	}
+
+	pthread_mutex_unlock(&task_mutex);
+
+	return prog_name;
+}
+
+int px4_prctl(int option, const char *arg2, px4_task_t pid)
+{
+	int rv;
+
+	switch (option) {
+	case PR_SET_NAME:
+		// set the threads name
+#ifdef __PX4_DARWIN
+		rv = pthread_setname_np(arg2);
+#else
+		rv = pthread_setname_np(pthread_self(), arg2);
+#endif
+		break;
+
+	default:
+		rv = -1;
+		PX4_WARN("FAILED SETTING TASK NAME");
+		break;
+	}
+
+	return rv;
+}
+

platforms/posix/src/px4/common/px4_sem.cpp

@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_sem.cpp
+ *
+ * PX4 Middleware Wrapper Linux Implementation
+ */
+
+#include <px4_defines.h>
+#include <px4_middleware.h>
+#include <px4_workqueue.h>
+#include <px4_time.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <pthread.h>
+#include <errno.h>
+
+#if (defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) || defined(__PX4_POSIX)) && !defined(__PX4_QURT)
+
+#include <px4_posix.h>
+
+int px4_sem_init(px4_sem_t *s, int pshared, unsigned value)
+{
+	// We do not used the process shared arg
+	(void)pshared;
+	s->value = value;
+	pthread_cond_init(&(s->wait), nullptr);
+	pthread_mutex_init(&(s->lock), nullptr);
+
+#if !defined(__PX4_DARWIN)
+	// We want to use CLOCK_MONOTONIC if possible but we can't on macOS
+	// because it's not available.
+	pthread_condattr_t attr;
+	pthread_condattr_init(&attr);
+	pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
+	pthread_cond_init(&(s->wait), &attr);
+#endif
+
+	return 0;
+}
+
+int px4_sem_setprotocol(px4_sem_t *s, int protocol)
+{
+	return 0;
+}
+
+int px4_sem_wait(px4_sem_t *s)
+{
+	int ret = pthread_mutex_lock(&(s->lock));
+
+	if (ret) {
+		return ret;
+	}
+
+	s->value--;
+
+	if (s->value < 0) {
+		ret = pthread_cond_wait(&(s->wait), &(s->lock));
+
+	} else {
+		ret = 0;
+	}
+
+	if (ret) {
+		PX4_WARN("px4_sem_wait failure");
+	}
+
+	int mret = pthread_mutex_unlock(&(s->lock));
+
+	return (ret) ? ret : mret;
+}
+
+int px4_sem_trywait(px4_sem_t *s)
+{
+	int ret = pthread_mutex_lock(&(s->lock));
+
+	if (ret) {
+		return ret;
+	}
+
+	if (s->value <= 0) {
+		errno = EAGAIN;
+		ret = -1;
+
+	} else {
+		s->value--;
+	}
+
+	int mret = pthread_mutex_unlock(&(s->lock));
+
+	return (ret) ? ret : mret;
+}
+
+int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime)
+{
+	int ret = pthread_mutex_lock(&(s->lock));
+
+	if (ret) {
+		return ret;
+	}
+
+	s->value--;
+	errno = 0;
+
+	if (s->value < 0) {
+		ret = px4_pthread_cond_timedwait(&(s->wait), &(s->lock), abstime);
+
+	} else {
+		ret = 0;
+	}
+
+	errno = ret;
+
+	if (ret != 0 && ret != ETIMEDOUT) {
+		setbuf(stdout, nullptr);
+		setbuf(stderr, nullptr);
+		const unsigned NAMELEN = 32;
+		char thread_name[NAMELEN] = {};
+		(void)pthread_getname_np(pthread_self(), thread_name, NAMELEN);
+		PX4_WARN("%s: px4_sem_timedwait failure: ret: %d", thread_name, ret);
+	}
+
+	int mret = pthread_mutex_unlock(&(s->lock));
+
+	if (ret || mret) {
+		return -1;
+	}
+
+	return 0;
+}
+
+int px4_sem_post(px4_sem_t *s)
+{
+	int ret = pthread_mutex_lock(&(s->lock));
+
+	if (ret) {
+		return ret;
+	}
+
+	s->value++;
+
+	if (s->value <= 0) {
+		ret = pthread_cond_signal(&(s->wait));
+
+	} else {
+		ret = 0;
+	}
+
+	if (ret) {
+		PX4_WARN("px4_sem_post failure");
+	}
+
+	int mret = pthread_mutex_unlock(&(s->lock));
+
+	// return the cond signal failure if present,
+	// else return the mutex status
+	return (ret) ? ret : mret;
+}
+
+int px4_sem_getvalue(px4_sem_t *s, int *sval)
+{
+	int ret = pthread_mutex_lock(&(s->lock));
+
+	if (ret) {
+		PX4_WARN("px4_sem_getvalue failure");
+	}
+
+	if (ret) {
+		return ret;
+	}
+
+	*sval = s->value;
+	ret = pthread_mutex_unlock(&(s->lock));
+
+	return ret;
+}
+
+int px4_sem_destroy(px4_sem_t *s)
+{
+	pthread_mutex_lock(&(s->lock));
+	pthread_cond_destroy(&(s->wait));
+	pthread_mutex_unlock(&(s->lock));
+	pthread_mutex_destroy(&(s->lock));
+
+	return 0;
+}
+
+#endif

platforms/posix/src/px4/common/shmem_posix.cpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2015 Vijay Venkatraman. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <errno.h>
+#include <semaphore.h>
+
+#include <sys/stat.h>
+
+#include <drivers/drv_hrt.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+#include <parameters/param.h>
+
+#include <shmem.h>
+#include "px4muorb.h"
+
+//#define SHMEM_DEBUG
+
+static uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0;
+extern unsigned char *adsp_changed_index;
+
+struct param_wbuf_s {
+	union param_value_u val;
+	param_t param;
+	bool unsaved;
+};
+
+/*update value and param's change bit in shared memory*/
+void update_to_shmem(param_t param, union param_value_u value)
+{
+	if (px4muorb_param_update_to_shmem(param, (unsigned char *) &value, sizeof(value))) {
+		PX4_ERR("krait update param %u failed", param);
+	}
+}
+
+void update_index_from_shmem(void)
+{
+	if (!adsp_changed_index) {
+		PX4_ERR("%s no param buffer", __FUNCTION__);
+		return;
+	}
+
+	px4muorb_param_update_index_from_shmem(adsp_changed_index, PARAM_BUFFER_SIZE);
+}
+
+static void update_value_from_shmem(param_t param, union param_value_u *value)
+{
+	if (px4muorb_param_update_value_from_shmem(param, (unsigned char *) value, sizeof(union param_value_u))) {
+		PX4_ERR("%s get param failed", __FUNCTION__);
+	}
+}
+
+int update_from_shmem(param_t param, union param_value_u *value)
+{
+	unsigned int byte_changed, bit_changed;
+	unsigned int retval = 0;
+
+	if (!adsp_changed_index) {
+		PX4_ERR("%s no param buffer", __FUNCTION__);
+		return 0;
+	}
+
+	update_from_shmem_current_time = hrt_absolute_time();
+
+	if ((update_from_shmem_current_time - update_from_shmem_prev_time)
+	    > 1000000) { //update every 1 second
+		update_from_shmem_prev_time = update_from_shmem_current_time;
+		update_index_from_shmem();
+	}
+
+	byte_changed = param / 8;
+	bit_changed = 1 << param % 8;
+
+	if (adsp_changed_index[byte_changed] & bit_changed) {
+		update_value_from_shmem(param, value);
+		adsp_changed_index[byte_changed] &= ~bit_changed; //clear the bit
+		retval = 1;
+	}
+
+	//else {PX4_INFO("no change to param %s", param_name(param));}
+
+	PX4_DEBUG("%s %d bit on adsp index[%d]",
+		  (retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
+
+	return retval;
+}