Merge branch 'fmuv2_bringup' of github.com:cvg/Firmware_Private into fmuv2_bringup

13 files changed, 353 insertions, 224 deletions

diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 5d485b01f..064426f21 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,11 +43,7 @@
 #define CONVERSIONS_H_
 #include <float.h>
 #include <stdint.h>
-
-#define CONSTANTS_ONE_G						9.80665f		// m/s^2
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f			// kg/m^3
-#define CONSTANTS_AIR_GAS_CONST				287.1f 			// J/(kg * K)
-#define CONSTANTS_ABSOLUTE_NULL_CELSIUS		-273.15f		// °C
+#include <systemlib/geo/geo.h>
 
 __BEGIN_DECLS
 
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 20b711fa6..afc5b072c 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -1,9 +1,8 @@
 /****************************************************************************
- * configs/px4fmu/src/up_leds.c
- * arch/arm/src/board/up_leds.c
  *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ * 	     Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +14,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -34,18 +33,26 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
+/**
+ * @file cpuload.c
+ *
+ * Measurement of CPU load of each individual task.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ */
 #include <nuttx/config.h>
+#include <nuttx/sched.h>
 
+#include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
+
+#include <arch/arch.h>
+
 #include <debug.h>
 
 #include <sys/time.h>
-#include <sched.h>
 
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
@@ -54,31 +61,16 @@
 
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 
-/****************************************************************************
- * Definitions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-__EXPORT void sched_note_start(FAR _TCB *tcb);
-__EXPORT void sched_note_stop(FAR _TCB *tcb);
-__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb);
-
-/****************************************************************************
- * Name:
- ****************************************************************************/
+__EXPORT void sched_note_start(FAR struct tcb_s *tcb);
+__EXPORT void sched_note_stop(FAR struct tcb_s *tcb);
+__EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb);
 
 __EXPORT struct system_load_s system_load;
 
-extern FAR _TCB *sched_gettcb(pid_t pid);
+extern FAR struct _TCB *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
-//	if (!system_load.initialized)
-//	{
 	system_load.start_time = hrt_absolute_time();
 	int i;
 
@@ -86,30 +78,32 @@ void cpuload_initialize_once()
 		system_load.tasks[i].valid = false;
 	}
 
-	system_load.total_count = 0;
-
 	uint64_t now = hrt_absolute_time();
 
-	/* initialize idle thread statically */
-	system_load.tasks[0].start_time = now;
-	system_load.tasks[0].total_runtime = 0;
-	system_load.tasks[0].curr_start_time = 0;
-	system_load.tasks[0].tcb = sched_gettcb(0);
-	system_load.tasks[0].valid = true;
-	system_load.total_count++;
-
-	/* initialize init thread statically */
-	system_load.tasks[1].start_time = now;
-	system_load.tasks[1].total_runtime = 0;
-	system_load.tasks[1].curr_start_time = 0;
-	system_load.tasks[1].tcb = sched_gettcb(1);
-	system_load.tasks[1].valid = true;
-	/* count init thread */
-	system_load.total_count++;
-	//	}
+	int static_tasks_count = 2;	// there are at least 2 threads that should be initialized statically - "idle" and "init"
+
+#ifdef CONFIG_PAGING
+	static_tasks_count++;	// include paging thread in initialization
+#endif /* CONFIG_PAGING */
+#if CONFIG_SCHED_WORKQUEUE
+	static_tasks_count++;	// include high priority work0 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+#if CONFIG_SCHED_LPWORK
+	static_tasks_count++;	// include low priority work1 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+
+	// perform static initialization of "system" threads
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
+	{
+		system_load.tasks[system_load.total_count].start_time = now;
+		system_load.tasks[system_load.total_count].total_runtime = 0;
+		system_load.tasks[system_load.total_count].curr_start_time = 0;
+		system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count);	// it is assumed that these static threads have consecutive PIDs
+		system_load.tasks[system_load.total_count].valid = true;
+	}
 }
 
-void sched_note_start(FAR _TCB *tcb)
+void sched_note_start(FAR struct tcb_s *tcb)
 {
 	/* search first free slot */
 	int i;
@@ -128,7 +122,7 @@ void sched_note_start(FAR _TCB *tcb)
 	}
 }
 
-void sched_note_stop(FAR _TCB *tcb)
+void sched_note_stop(FAR struct tcb_s *tcb)
 {
 	int i;
 
@@ -145,7 +139,7 @@ void sched_note_stop(FAR _TCB *tcb)
 	}
 }
 
-void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
+void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb)
 {
 	uint64_t new_time = hrt_absolute_time();
 
diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h
index a97047ea8..c7aa18d3c 100644
--- a/src/modules/systemlib/cpuload.h
+++ b/src/modules/systemlib/cpuload.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -43,7 +43,7 @@ struct system_load_taskinfo_s {
 	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
 	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
 	uint64_t start_time;         ///< FIRST start time of task
-	FAR struct _TCB *tcb;               ///<
+	FAR struct tcb_s *tcb;               ///<
 	bool valid;                  ///< Task is currently active / valid
 };
 
@@ -60,4 +60,6 @@ __EXPORT extern struct system_load_s system_load;
 
 __EXPORT void cpuload_initialize_once(void);
 
+__END_DECLS
+
 #endif
diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index daf17ef8b..6c0e876d1 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -61,7 +61,7 @@ const char *
 getprogname(void)
 {
 #if CONFIG_TASK_NAME_SIZE > 0
-	_TCB	*thisproc = sched_self();
+	FAR struct tcb_s	*thisproc = sched_self();
 
 	return thisproc->name;
 #else
diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
index 84097b49f..dadec51ec 100644
--- a/src/modules/systemlib/geo/geo.h
+++ b/src/modules/systemlib/geo/geo.h
@@ -45,9 +45,23 @@
  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
 
+#pragma once
+
+__BEGIN_DECLS
 
 #include <stdbool.h>
 
+#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
+#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
+
+/* compatibility aliases */
+#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
+#define GRAVITY_MSS		CONSTANTS_ONE_G
+
+// XXX remove
 struct crosstrack_error_s {
 	bool past_end;		// Flag indicating we are past the end of the line/arc segment
 	float distance;		// Distance in meters to closest point on line/arc
@@ -111,3 +125,5 @@ __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
+
+__END_DECLS
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 88f7f762c..8e9c2bfcf 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -53,14 +53,26 @@
 
 
 struct hx_stream {
-	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
-	unsigned		frame_bytes;
-	bool			escaped;
-	bool			txerror;
-
+	/* RX state */
+	uint8_t			rx_buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		rx_frame_bytes;
+	bool			rx_escaped;
+	hx_stream_rx_callback	rx_callback;
+	void			*rx_callback_arg;
+
+	/* TX state */
 	int			fd;
-	hx_stream_rx_callback	callback;
-	void			*callback_arg;
+	bool			tx_error;
+	uint8_t			*tx_buf;
+	unsigned		tx_resid;
+	uint32_t		tx_crc;
+	enum {
+		TX_IDLE = 0,
+		TX_SEND_START,
+		TX_SEND_DATA,
+		TX_SENT_ESCAPE,
+		TX_SEND_END
+	}			tx_state;
 
 	perf_counter_t		pc_tx_frames;
 	perf_counter_t		pc_rx_frames;
@@ -76,27 +88,12 @@ struct hx_stream {
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static int	hx_rx_frame(hx_stream_t stream);
-static bool	hx_rx_char(hx_stream_t stream, uint8_t c);
 
 static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
 {
 	if (write(stream->fd, &c, 1) != 1)
-		stream->txerror = true;
-}
-
-static void
-hx_tx_byte(hx_stream_t stream, uint8_t c)
-{
-	switch (c) {
-	case FBO:
-	case CEO:
-		hx_tx_raw(stream, CEO);
-		c ^= 0x20;
-		break;
-	}
-
-	hx_tx_raw(stream, c);
+		stream->tx_error = true;
 }
 
 static int
@@ -106,11 +103,11 @@ hx_rx_frame(hx_stream_t stream)
 		uint8_t	b[4];
 		uint32_t w;
 	} u;
-	unsigned length = stream->frame_bytes;
+	unsigned length = stream->rx_frame_bytes;
 
 	/* reset the stream */
-	stream->frame_bytes = 0;
-	stream->escaped = false;
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
 
 	/* not a real frame - too short */
 	if (length < 4) {
@@ -123,11 +120,11 @@ hx_rx_frame(hx_stream_t stream)
 	length -= 4;
 
 	/* compute expected CRC */
-	u.w = crc32(&stream->buf[0], length);
+	u.w = crc32(&stream->rx_buf[0], length);
 
 	/* compare computed and actual CRC */
 	for (unsigned i = 0; i < 4; i++) {
-		if (u.b[i] != stream->buf[length + i]) {
+		if (u.b[i] != stream->rx_buf[length + i]) {
 			perf_count(stream->pc_rx_errors);
 			return 0;
 		}
@@ -135,7 +132,7 @@ hx_rx_frame(hx_stream_t stream)
 
 	/* frame is good */
 	perf_count(stream->pc_rx_frames);
-	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
 	return 1;
 }
 
@@ -151,8 +148,8 @@ hx_stream_init(int fd,
 	if (stream != NULL) {
 		memset(stream, 0, sizeof(struct hx_stream));
 		stream->fd = fd;
-		stream->callback = callback;
-		stream->callback_arg = arg;
+		stream->rx_callback = callback;
+		stream->rx_callback_arg = arg;
 	}
 
 	return stream;
@@ -180,105 +177,135 @@ hx_stream_set_counters(hx_stream_t stream,
 	stream->pc_rx_errors = rx_errors;
 }
 
+void
+hx_stream_reset(hx_stream_t stream)
+{
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
+
+	stream->tx_buf = NULL;
+	stream->tx_resid = 0;
+	stream->tx_state = TX_IDLE;
+}
+
 int
-hx_stream_send(hx_stream_t stream,
+hx_stream_start(hx_stream_t stream,
 	       const void *data,
 	       size_t count)
 {
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} u;
-	const uint8_t *p = (const uint8_t *)data;
-	unsigned resid = count;
-
-	if (resid > HX_STREAM_MAX_FRAME)
+	if (count > HX_STREAM_MAX_FRAME)
 		return -EINVAL;
 
-	/* start the frame */
-	hx_tx_raw(stream, FBO);
+	stream->tx_buf = data;
+	stream->tx_resid = count;
+	stream->tx_state = TX_SEND_START;
+	stream->tx_crc = crc32(data, count);
+	return OK;
+}
 
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
 
-	/* compute the CRC */
-	u.w = crc32(data, count);
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
 
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
 
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
 
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+		switch (c) {
+		case FBO:
+		case CEO:
+			stream->tx_state = TX_SENT_ESCAPE;
+			return CEO;
+		}
+		break;
+
+	case TX_SENT_ESCAPE:
+		c = *stream->tx_buf ^ 0x20;
+		stream->tx_state = TX_SEND_DATA;
+		break;
+
+	case TX_SEND_END:
+		stream->tx_state = TX_IDLE;
+		return FBO;
+
+	case TX_IDLE:
+	default:
+		return -1;
+	}
+
+	/* if we are here, we have consumed a byte from the buffer */
+	stream->tx_resid--;
+	stream->tx_buf++;
+
+	/* buffer exhausted */
+	if (stream->tx_resid == 0) {
+		uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+		/* was the buffer the frame CRC? */
+		if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+			stream->tx_state = TX_SEND_END;
+		} else {
+			/* no, it was the payload - switch to sending the CRC */
+			stream->tx_buf = pcrc;
+			stream->tx_resid = sizeof(stream->tx_crc);
+		}
+	}
+	return c;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	int result;
+
+	result = hx_stream_start(stream, data, count);
+	if (result != OK)
+		return result;
+
+	int c;
+	while ((c = hx_stream_send_next(stream)) >= 0)
+		hx_tx_raw(stream, c);
 
 	/* check for transmit error */
-	if (stream->txerror) {
-		stream->txerror = false;
+	if (stream->tx_error) {
+		stream->tx_error = false;
 		return -EIO;
 	}
 
 	perf_count(stream->pc_tx_frames);
-	return 0;
+	return OK;
 }
 
-static bool
-hx_rx_char(hx_stream_t stream, uint8_t c)
+void
+hx_stream_rx(hx_stream_t stream, uint8_t c)
 {
 	/* frame end? */
 	if (c == FBO) {
 		hx_rx_frame(stream);
-		return true;
+		return;
 	}
 
 	/* escaped? */
-	if (stream->escaped) {
-		stream->escaped = false;
+	if (stream->rx_escaped) {
+		stream->rx_escaped = false;
 		c ^= 0x20;
 
 	} else if (c == CEO) {
-		/* now escaped, ignore the byte */
-		stream->escaped = true;
-		return false;
+		/* now rx_escaped, ignore the byte */
+		stream->rx_escaped = true;
+		return;
 	}
 
 	/* save for later */
-	if (stream->frame_bytes < sizeof(stream->buf))
-		stream->buf[stream->frame_bytes++] = c;
-
-	return false;
-}
-
-void
-hx_stream_rx_char(hx_stream_t stream, uint8_t c)
-{
-	hx_rx_char(stream, c);
-}
-
-int
-hx_stream_rx(hx_stream_t stream)
-{
-	uint16_t	buf[16];
-	ssize_t		len;
-
-	/* read bytes */
-	len = read(stream->fd, buf, sizeof(buf));
-	if (len <= 0) {
-
-		/* nonblocking stream and no data */
-		if (errno == EWOULDBLOCK)
-			return 0;
-
-		/* error or EOF */
-		return -errno;
-	}
-
-	/* process received characters */
-	for (int i = 0; i < len; i++)
-		hx_rx_char(stream, buf[i]);
-
-	return 0;
+	if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+		stream->rx_buf[stream->rx_frame_bytes++] = c;
 }
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index be4850f74..1f3927222 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -58,7 +58,8 @@ __BEGIN_DECLS
  * Allocate a new hx_stream object.
  *
  * @param fd		The file handle over which the protocol will
- *			communicate.
+ *			communicate, or -1 if the protocol will use
+ *			hx_stream_start/hx_stream_send_next.
  * @param callback	Called when a frame is received.
  * @param callback_arg	Passed to the callback.
  * @return		A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void		hx_stream_free(hx_stream_t stream);
  *
  * Any counter may be set to NULL to disable counting that datum.
  *
+ * @param stream	A handle returned from hx_stream_init.
  * @param tx_frames	Counter for transmitted frames.
  * @param rx_frames	Counter for received frames.
  * @param rx_errors	Counter for short and corrupt received frames.
@@ -90,6 +92,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_errors);
 
 /**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to 
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_start(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		The byte to send, or -1 if there is 
+ *			nothing left to send.
+ */
+__EXPORT extern int		hx_stream_send_next(hx_stream_t stream);
+
+/**
  * Send a frame.
  *
  * This function will block until all frame bytes are sent if
@@ -114,25 +154,9 @@ __EXPORT extern int		hx_stream_send(hx_stream_t stream,
  * @param stream	A handle returned from hx_stream_init.
  * @param c		The character to process.
  */
-__EXPORT extern void		hx_stream_rx_char(hx_stream_t stream,
+__EXPORT extern void		hx_stream_rx(hx_stream_t stream,
 		uint8_t c);
 
-/**
- * Handle received bytes from the stream.
- *
- * Note that this interface should only be used with blocking streams
- * when it is OK for the call to block until a frame is received.
- *
- * When used with a non-blocking stream, it will typically return
- * immediately, or after handling a received frame.
- *
- * @param stream	A handle returned from hx_stream_init.
- * @return		-errno on error, nonzero if a frame
- *			has been received, or if not enough
- *			bytes are available to complete a frame.
- */
-__EXPORT extern int		hx_stream_rx(hx_stream_t stream);
-
 __END_DECLS
 
 #endif
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 879f4715a..3c1e10287 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -201,23 +201,50 @@ perf_end(perf_counter_t handle)
 	switch (handle->type) {
 	case PC_ELAPSED: {
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
-			hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			pce->event_count++;
-			pce->time_total += elapsed;
+			if (pce->time_start != 0) {
+				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
 
-			if ((pce->time_least > elapsed) || (pce->time_least == 0))
-				pce->time_least = elapsed;
+				pce->event_count++;
+				pce->time_total += elapsed;
 
-			if (pce->time_most < elapsed)
-				pce->time_most = elapsed;
+				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+					pce->time_least = elapsed;
+
+				if (pce->time_most < elapsed)
+					pce->time_most = elapsed;
+
+				pce->time_start = 0;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+perf_cancel(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return;
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			pce->time_start = 0;
 		}
+		break;
 
 	default:
 		break;
 	}
 }
 
+
+
 void
 perf_reset(perf_counter_t handle)
 {
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 5c2cb15b2..4cd8b67a1 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -92,13 +92,25 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
  */
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
 /**
- * Reset a performance event.
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ */
+__EXPORT extern void		perf_cancel(perf_counter_t handle);
+
+/**
+ * Reset a performance counter.
  *
  * This call resets performance counter to initial state
  *
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 49315cdc9..4996a8f66 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,14 +38,21 @@
 
 /**
  * @file pid.c
- * Implementation of generic PID control interface
+ *
+ * Implementation of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include "pid.h"
 #include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-		       float limit, uint8_t mode)
+		       float limit, uint8_t mode, float dt_min)
 {
 	pid->kp = kp;
 	pid->ki = ki;
@@ -51,13 +60,13 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->intmax = intmax;
 	pid->limit = limit;
 	pid->mode = mode;
-	pid->count = 0;
-	pid->saturated = 0;
-	pid->last_output = 0;
-
-	pid->sp = 0;
-	pid->error_previous = 0;
-	pid->integral = 0;
+	pid->dt_min = dt_min;
+	pid->count = 0.0f;
+	pid->saturated = 0.0f;
+	pid->last_output = 0.0f;
+	pid->sp = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->integral = 0.0f;
 }
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
 {
@@ -136,14 +145,14 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculated current error value
 	float error = pid->sp - val;
 
-	if (isfinite(error)) {				// Why is this necessary?  DEW
-		pid->error_previous = error;
-	}
-
 	// Calculate or measured current error derivative
-
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / dt;
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
 
 	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
 		d = -val_dot;
@@ -152,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
+
 	// Calculate the error integral and check for saturation
 	i = pid->integral + (error * dt);
 
@@ -162,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 	} else {
 		if (!isfinite(i)) {
-			i = 0;
+			i = 0.0f;
 		}
 
 		pid->integral = i;
@@ -170,17 +183,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	}
 
 	// Calculate the output.  Limit output magnitude to pid->limit
-	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
-	if (output > pid->limit) output = pid->limit;
+	if (isfinite(output)) {
+		if (output > pid->limit) {
+			output = pid->limit;
 
-	if (output < -pid->limit) output = -pid->limit;
+		} else if (output < -pid->limit) {
+			output = -pid->limit;
+		}
 
-	if (isfinite(output)) {
 		pid->last_output = output;
 	}
 
-
 	return pid->last_output;
 }
 
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index 64d668867..eca228464 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +38,14 @@
 
 /**
  * @file pid.h
- * Definition of generic PID control interface
+ *
+ * Definition of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef PID_H_
@@ -44,11 +53,16 @@
 
 #include <stdint.h>
 
+__BEGIN_DECLS
+
 /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
  * val_dot in pid_calculate() will be ignored */
 #define PID_MODE_DERIVATIV_CALC	0
+/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
+ * val_dot in pid_calculate() will be ignored */
+#define PID_MODE_DERIVATIV_CALC_NO_SP	1
 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-#define PID_MODE_DERIVATIV_SET	1
+#define PID_MODE_DERIVATIV_SET	2
 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
 #define PID_MODE_DERIVATIV_NONE 9
 
@@ -62,17 +76,19 @@ typedef struct {
 	float error_previous;
 	float last_output;
 	float limit;
+	float dt_min;
 	uint8_t mode;
 	uint8_t count;
 	uint8_t saturated;
 } PID_t;
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
 
 __EXPORT void pid_reset_integral(PID_t *pid);
 
+__END_DECLS
 
 #endif /* PID_H_ */
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index afb7eca29..3283aad8a 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -50,7 +50,7 @@
 
 #include "systemlib.h"
 
-static void kill_task(FAR _TCB *tcb, FAR void *arg);
+static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
 
 void killall()
 {
@@ -60,12 +60,12 @@ void killall()
 	sched_foreach(kill_task, NULL);
 }
 
-static void kill_task(FAR _TCB *tcb, FAR void *arg)
+static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
 {
 	kill(tcb->pid, SIGUSR1);
 }
 
-int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
 {
 	int pid;
 
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 2c53c648b..0194b5e52 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
 #include <float.h>
 #include <stdint.h>
 
-__BEGIN_DECLS
-
 /** Reboots the board */
 extern void up_systemreset(void) noreturn_function;
 
+__BEGIN_DECLS
+
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
@@ -58,7 +58,7 @@ __EXPORT void killall(void);
 #endif
 
 /** Starts a task and performs any specific accounting, scheduler setup, etc. */
-__EXPORT int task_spawn(const char *name,
+__EXPORT int task_spawn_cmd(const char *name,
 			int priority,
 			int scheduler,
 			int stack_size,