From 97e4909d9e7c835b73c2eebdf4991d395c4ffb3f Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 Aug 2013 19:40:57 -0700
Subject: Improvements to the HX stream protocol implementation.

---
 src/modules/systemlib/hx_stream.c | 185 +++++++++++++++++++++++++-------------
 src/modules/systemlib/hx_stream.h |  42 ++++++++-
 2 files changed, 164 insertions(+), 63 deletions(-)

(limited to 'src')

diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8d77f14a8..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;
@@ -81,21 +93,7 @@ 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
@@ -105,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) {
@@ -122,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;
 		}
@@ -134,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;
 }
 
@@ -150,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;
@@ -179,49 +177,112 @@ 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;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
+
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
 
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
 
-	/* compute the CRC */
-	u.w = crc32(data, count);
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
 
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
+		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;
 
-	while (resid--)
-		hx_tx_byte(stream, *p++);
+	result = hx_stream_start(stream, data, count);
+	if (result != OK)
+		return result;
 
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+	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;
 }
 
 void
@@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
 	}
 
 	/* 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;
+		/* 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;
+	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 128689953..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.
@@ -89,6 +91,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_frames,
 		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.
  *
-- 
cgit v1.2.3


From c33048b52186b88ddab2a9c9fdad24c7b64e7e22 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 Aug 2013 19:42:16 -0700
Subject: Add the ability to cancel a begin/end perf counter if the begin turns
 out to have been in error.

---
 src/modules/systemlib/perf_counter.c | 41 ++++++++++++++++++++++++++++++------
 src/modules/systemlib/perf_counter.h | 14 +++++++++++-
 2 files changed, 47 insertions(+), 8 deletions(-)

(limited to 'src')

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
  *
-- 
cgit v1.2.3


From 567f621754f1f68ed0aae560e9590805045fa3e0 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 Aug 2013 19:43:05 -0700
Subject: Fix an issue with the pwm_servo driver when using one of the STM32
 advanced timers.

---
 src/drivers/stm32/drv_pwm_servo.c | 6 ++++++
 1 file changed, 6 insertions(+)

(limited to 'src')

diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index 7b060412c..dbb45a138 100644
--- a/src/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -88,6 +88,7 @@
 #define rCCR4(_tmr)   	REG(_tmr, STM32_GTIM_CCR4_OFFSET)
 #define rDCR(_tmr)    	REG(_tmr, STM32_GTIM_DCR_OFFSET)
 #define rDMAR(_tmr)   	REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+#define rBDTR(_tmr)	REG(_tmr, STM32_ATIM_BDTR_OFFSET)
 
 static void		pwm_timer_init(unsigned timer);
 static void		pwm_timer_set_rate(unsigned timer, unsigned rate);
@@ -110,6 +111,11 @@ pwm_timer_init(unsigned timer)
 	rCCER(timer) = 0;
 	rDCR(timer) = 0;
 
+	if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) {
+		/* master output enable = on */
+		rBDTR(timer) = ATIM_BDTR_MOE;
+	}
+
 	/* configure the timer to free-run at 1MHz */
 	rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
 
-- 
cgit v1.2.3


From 4b4f33e6a4fb18047a6ca73d3a4872a900519b5c Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 Aug 2013 19:48:20 -0700
Subject: Add direct-access methods to the base Device class, so that there's a
 common way of talking to drivers regardless of which of the specialised
 classes they derive from.

Make the Device destructor public and virtual, so that arbitrary devices can be deleted. Likewise for classes that derive from it.

Make Device::init public so that arbitrary devices can be initialised after being returned by factories.
---
 src/drivers/device/device.cpp | 19 +++++++++++-
 src/drivers/device/device.h   | 70 +++++++++++++++++++++++++++++++++++--------
 src/drivers/device/i2c.h      | 14 ++++-----
 src/drivers/device/spi.h      |  2 +-
 4 files changed, 82 insertions(+), 23 deletions(-)

(limited to 'src')

diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index 04a5222c3..c3ee77b1c 100644
--- a/src/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
@@ -223,5 +223,22 @@ interrupt(int irq, void *context)
 	return OK;
 }
 
+int
+Device::read(unsigned offset, void *data, unsigned count)
+{
+	return -ENODEV;
+}
+
+int
+Device::write(unsigned offset, void *data, unsigned count)
+{
+	return -ENODEV;
+}
+
+int
+Device::ioctl(unsigned operation, unsigned &arg)
+{
+	return -ENODEV;
+}
 
-} // namespace device
\ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 7d375aab9..a9ed5d77c 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -68,11 +68,62 @@ namespace device __EXPORT
 class __EXPORT Device
 {
 public:
+	/**
+	 * Destructor.
+	 *
+	 * Public so that anonymous devices can be destroyed.
+	 */
+	virtual ~Device();
+
 	/**
 	 * Interrupt handler.
 	 */
 	virtual void	interrupt(void *ctx);	/**< interrupt handler */
 
+	/*
+	 * Direct access methods.
+	 */
+
+	/**
+	 * Initialise the driver and make it ready for use.
+	 *
+	 * @return	OK if the driver initialized OK, negative errno otherwise;
+	 */
+	virtual int	init();
+
+	/**
+	 * Read directly from the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param offset	The device address at which to start reading
+	 * @param data		The buffer into which the read values should be placed.
+	 * @param count		The number of items to read.
+	 * @return		The number of items read on success, negative errno otherwise.
+	 */
+	virtual int	read(unsigned address, void *data, unsigned count);
+
+	/**
+	 * Write directly to the device.
+	 *
+	 * The actual size of each unit quantity is device-specific.
+	 *
+	 * @param address	The device address at which to start writing.
+	 * @param data		The buffer from which values should be read.
+	 * @param count		The number of items to write.
+	 * @return		The number of items written on success, negative errno otherwise.
+	 */	 
+	virtual int	write(unsigned address, void *data, unsigned count);
+
+	/**
+	 * Perform a device-specific operation.
+	 *
+	 * @param operation	The operation to perform.
+	 * @param arg		An argument to the operation.
+	 * @return		Negative errno on error, OK or positive value on success.
+	 */
+	virtual int	ioctl(unsigned operation, unsigned &arg);
+
 protected:
 	const char	*_name;			/**< driver name */
 	bool		_debug_enabled;		/**< if true, debug messages are printed */
@@ -85,14 +136,6 @@ protected:
 	 */
 	Device(const char *name,
 	       int irq = 0);
-	~Device();
-
-	/**
-	 * Initialise the driver and make it ready for use.
-	 *
-	 * @return	OK if the driver initialised OK.
-	 */
-	virtual int	init();
 
 	/**
 	 * Enable the device interrupt
@@ -189,7 +232,7 @@ public:
 	/**
 	 * Destructor
 	 */
-	~CDev();
+	virtual ~CDev();
 
 	virtual int	init();
 
@@ -282,6 +325,7 @@ public:
 	 * Test whether the device is currently open.
 	 *
 	 * This can be used to avoid tearing down a device that is still active.
+	 * Note - not virtual, cannot be overridden by a subclass.
 	 *
 	 * @return              True if the device is currently open.
 	 */
@@ -396,9 +440,9 @@ public:
 	    const char *devname,
 	    uint32_t base,
 	    int irq = 0);
-	~PIO();
+	virtual ~PIO();
 
-	int		init();
+	virtual int	init();
 
 protected:
 
@@ -407,7 +451,7 @@ protected:
 	 *
 	 * @param offset	Register offset in bytes from the base address.
 	 */
-	uint32_t reg(uint32_t offset) {
+	uint32_t	reg(uint32_t offset) {
 		return *(volatile uint32_t *)(_base + offset);
 	}
 
@@ -444,4 +488,4 @@ private:
 
 } // namespace device
 
-#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file
+#endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index b4a9cdd53..549879352 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -55,13 +55,11 @@ class __EXPORT I2C : public CDev
 
 public:
 
-    /**
-     * Get the address
-     */
-    uint16_t get_address() {
-      return _address;
-    }
-
+	/**
+	 * Get the address
+	 */
+	int16_t		get_address() { return _address; }
+	
 protected:
 	/**
 	 * The number of times a read or write operation will be retried on
@@ -90,7 +88,7 @@ protected:
 	    uint16_t address,
 	    uint32_t frequency,
 	    int irq = 0);
-	~I2C();
+	virtual ~I2C();
 
 	virtual int	init();
 
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index d2d01efb3..e0122372a 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -71,7 +71,7 @@ protected:
 	    enum spi_mode_e mode,
 	    uint32_t frequency,
 	    int irq = 0);
-	~SPI();
+	virtual ~SPI();
 
 	virtual int	init();
 
-- 
cgit v1.2.3


From 1fb4705ab7038fb4b135b49c58f14b48f0cfab48 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 Aug 2013 19:48:46 -0700
Subject: Add size and flush methods to the ringbuffer class.

---
 src/drivers/device/ringbuffer.h | 21 ++++++++++++++++-----
 1 file changed, 16 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h
index 37686fdbe..dc0c84052 100644
--- a/src/drivers/device/ringbuffer.h
+++ b/src/drivers/device/ringbuffer.h
@@ -104,6 +104,17 @@ public:
 	 */
 	bool			full() { return _next(_head) == _tail; }
 
+	/*
+	 * Returns the capacity of the buffer, or zero if the buffer could
+	 * not be allocated.
+	 */
+	unsigned		size() { return (_buf != nullptr) ? _size : 0; }
+
+	/*
+	 * Empties the buffer.
+	 */
+	void			flush() { _head = _tail = _size; }
+
 private:
 	T			*const _buf;	
 	const unsigned		_size;
@@ -114,11 +125,11 @@ private:
 };
 
 template <typename T>
-RingBuffer<T>::RingBuffer(unsigned size) :
-	_buf(new T[size + 1]),
-	_size(size),
- 	_head(size),
- 	_tail(size)
+RingBuffer<T>::RingBuffer(unsigned with_size) :
+	_buf(new T[with_size + 1]),
+	_size(with_size),
+ 	_head(with_size),
+ 	_tail(with_size)
 {}
 
 template <typename T>
-- 
cgit v1.2.3


From 1acbbe46d178c7eb2d11a9ba3349d274a3dd1581 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 4 Aug 2013 19:49:19 -0700
Subject: Make it possible to create a cdev without automatically creating a
 device node.

---
 src/drivers/device/cdev.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

(limited to 'src')

diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 422321850..47ebcd40a 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -111,21 +111,21 @@ CDev::~CDev()
 int
 CDev::init()
 {
-	int ret = OK;
-
 	// base class init first
-	ret = Device::init();
+	int ret = Device::init();
 
 	if (ret != OK)
 		goto out;
 
 	// now register the driver
-	ret = register_driver(_devname, &fops, 0666, (void *)this);
+	if (_devname != nullptr) {
+		ret = register_driver(_devname, &fops, 0666, (void *)this);
 
-	if (ret != OK)
-		goto out;
+		if (ret != OK)
+			goto out;
 
-	_registered = true;
+		_registered = true;
+	}
 
 out:
 	return ret;
@@ -395,4 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
 	return cdev->poll(filp, fds, setup);
 }
 
-} // namespace device
\ No newline at end of file
+} // namespace device
-- 
cgit v1.2.3


From 5ddbe24d8e5383a19f51957463211a4d8922d366 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 5 Aug 2013 12:26:31 +0200
Subject: Fixed code style for meas_airspeed.cpp

---
 src/drivers/meas_airspeed/meas_airspeed.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 7a2e22c01..15cae7d70 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -122,7 +122,7 @@ protected:
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
 MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
-	CONVERSION_INTERVAL)
+			CONVERSION_INTERVAL)
 {
 
 }
@@ -171,6 +171,7 @@ MEASAirspeed::collect()
 
 	if (status == 2) {
 		log("err: stale data");
+
 	} else if (status == 3) {
 		log("err: fault");
 	}
-- 
cgit v1.2.3


From 901a9c3e35456445465e7008d3c69b0bd3481e9e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 5 Aug 2013 12:44:20 +0200
Subject: Hotfix: MEAS Airspeed sensor fixes from Sarthak Kaingade

---
 src/drivers/meas_airspeed/meas_airspeed.cpp | 31 +++++++++++++++++++----------
 1 file changed, 20 insertions(+), 11 deletions(-)

(limited to 'src')

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 15cae7d70..ebae21cf7 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -34,6 +34,7 @@
 /**
  * @file meas_airspeed.cpp
  * @author Lorenz Meier
+ * @author Sarthak Kaingade
  * @author Simon Wilks
  *
  * Driver for the MEAS Spec series connected via I2C.
@@ -92,9 +93,6 @@
 
 /* Register address */
 #define ADDR_READ_MR			0x00	/* write to this address to start conversion */
-#define ADDR_READ_DF2			0x00	/* read from this address to read pressure only */
-#define ADDR_READ_DF3			0x01
-#define ADDR_READ_DF4			0x02	/* read from this address to read pressure and temp */
 
 /* Measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
@@ -160,7 +158,7 @@ MEASAirspeed::collect()
 
 	perf_begin(_sample_perf);
 
-	ret = transfer(nullptr, 0, &val[0], 2);
+	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
 		log("error reading from sensor: %d", ret);
@@ -176,21 +174,32 @@ MEASAirspeed::collect()
 		log("err: fault");
 	}
 
-	uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
+	//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
 	uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
 
-	diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
-	diff_pres_pa -= _diff_pres_offset;
+	// XXX leaving this in until new calculation method has been cross-checked
+	//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
+	//diff_pres_pa -= _diff_pres_offset;
+	int16_t dp_raw = 0, dT_raw = 0;
+	dp_raw = (val[0] << 8) + val[1];
+	dp_raw = 0x3FFF & dp_raw;
+	dT_raw = (val[2] << 8) + val[3];
+	dT_raw = (0xFFE0 & dT_raw) >> 5;
+	float temperature = ((200 * dT_raw) / 2047) - 50;
 
 	// XXX we may want to smooth out the readings to remove noise.
 
+	// Calculate differential pressure. As its centered around 8000
+	// and can go positive or negative, enforce absolute value
+	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+
 	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].temperature = temp;
-	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
+	_reports[_next_report].temperature = temperature;
+	_reports[_next_report].differential_pressure_pa = diff_press_pa;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
+		_reports[_next_report].max_differential_pressure_pa = diff_press_pa;
 	}
 
 	/* announce the airspeed if needed, just publish else */
-- 
cgit v1.2.3


From b5d19d08ea02bd31e27f4259302b30946e1f673d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 5 Aug 2013 21:05:08 +0200
Subject: Equipped MPU6k driver with Butterworth for accel and gyro

---
 src/drivers/mpu6000/mpu6000.cpp | 74 ++++++++++++++++++++++++++++++++++-------
 1 file changed, 62 insertions(+), 12 deletions(-)

(limited to 'src')

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 9dcb4be9e..be0a04028 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -70,6 +70,7 @@
 #include <drivers/device/ringbuffer.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #define DIR_READ			0x80
 #define DIR_WRITE			0x00
@@ -202,6 +203,13 @@ private:
 	unsigned		_sample_rate;
 	perf_counter_t		_sample_perf;
 
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -332,8 +340,14 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
 	_reads(0),
-	_sample_rate(500),
-	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
+	_sample_rate(1000),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
+	_accel_filter_x(1000, 30),
+	_accel_filter_y(1000, 30),
+	_accel_filter_z(1000, 30),
+	_gyro_filter_x(1000, 30),
+        _gyro_filter_y(1000, 30),
+        _gyro_filter_z(1000, 30)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -714,6 +728,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
+					// adjust filters
+					float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+
+
+					float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
+					_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+					_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+					_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
+
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
@@ -767,9 +794,17 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		_set_sample_rate(arg);
 		return OK;
 
-	case ACCELIOCSLOWPASS:
 	case ACCELIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)arg);
+		return _accel_filter_x.get_cutoff_freq();
+
+	case ACCELIOCSLOWPASS:
+		
+		// XXX decide on relationship of both filters
+		// i.e. disable the on-chip filter
+		//_set_dlpf_filter((uint16_t)arg);
+                _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+                _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+                _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		return OK;
 
 	case ACCELIOCSSCALE:
@@ -853,9 +888,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		_set_sample_rate(arg);
 		return OK;
 
-	case GYROIOCSLOWPASS:
 	case GYROIOCGLOWPASS:
-		_set_dlpf_filter((uint16_t)arg);
+		return _gyro_filter_x.get_cutoff_freq();
+	case GYROIOCSLOWPASS:
+		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		// XXX check relation to the internal lowpass
+		//_set_dlpf_filter((uint16_t)arg);
 		return OK;
 
 	case GYROIOCSSCALE:
@@ -1112,9 +1152,14 @@ MPU6000::measure()
 	arb.y_raw = report.accel_y;
 	arb.z_raw = report.accel_z;
 
-	arb.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	arb.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	arb.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	
+	arb.x = _accel_filter_x.apply(x_in_new);
+	arb.y = _accel_filter_y.apply(y_in_new);
+	arb.z = _accel_filter_z.apply(z_in_new);
+
 	arb.scaling = _accel_range_scale;
 	arb.range_m_s2 = _accel_range_m_s2;
 
@@ -1125,9 +1170,14 @@ MPU6000::measure()
 	grb.y_raw = report.gyro_y;
 	grb.z_raw = report.gyro_z;
 
-	grb.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	grb.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	grb.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	
+	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
+	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
+	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
+
 	grb.scaling = _gyro_range_scale;
 	grb.range_rad_s = _gyro_range_rad_s;
 
-- 
cgit v1.2.3


From a2f923b9a3bf403e3a9fcee39d87c7aecc28559d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 5 Aug 2013 21:05:34 +0200
Subject: Increased MPU6K poll and sampling rate to 1 KHz

---
 src/modules/sensors/sensors.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 0f1782fca..5dc23f5c1 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -763,11 +763,11 @@ Sensors::accel_init()
 
 		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
-		/* set the accel internal sampling rate up to at leat 500Hz */
-		ioctl(fd, ACCELIOCSSAMPLERATE, 500);
+		/* set the accel internal sampling rate up to at leat 1000Hz */
+		ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		/* set the driver to poll at 1000Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
 		#else
 
-- 
cgit v1.2.3


From cfd737aa734f9b0cd97f79148d6b959978b2cad0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 5 Aug 2013 21:08:19 +0200
Subject: Made sensors startup routine more flexible

---
 src/modules/sensors/sensors.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 5dc23f5c1..f7b41b120 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -801,11 +801,13 @@ Sensors::gyro_init()
 
 		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
-		/* set the gyro internal sampling rate up to at leat 500Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 500);
+		/* set the gyro internal sampling rate up to at least 1000Hz */
+		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
+			ioctl(fd, GYROIOCSSAMPLERATE, 800);
 
-		/* set the driver to poll at 500Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 500);
+		/* set the driver to poll at 1000Hz */
+		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
+			ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
 		#else
 
-- 
cgit v1.2.3


From 338e506a28e4233bc8a16493530f3b82a0dd67e9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 6 Aug 2013 16:33:01 +1000
Subject: mpu6000: set the default DLFP filter to 42Hz

this allows for apps to ask for slightly higher filters with the
software filter and not have it completely ruined by the on-chip DLPF
---
 src/drivers/mpu6000/mpu6000.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index be0a04028..c4e331a30 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -477,7 +477,7 @@ void MPU6000::reset()
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
 	// was 90 Hz, but this ruins quality and does not improve the
 	// system response
-	_set_dlpf_filter(20);
+	_set_dlpf_filter(42);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
-- 
cgit v1.2.3