Merged export-build

src/drivers/device/cdev.cpp

@@ -0,0 +1,398 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 cdev.cpp
+ *
+ * Character device base class.
+ */
+
+#include "device.h"
+
+#include <sys/ioctl.h>
+#include <arch/irq.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef CONFIG_DISABLE_POLL
+# error This driver is not compatible with CONFIG_DISABLE_POLL
+#endif
+
+namespace device
+{
+
+/* how much to grow the poll waiter set each time it has to be increased */
+static const unsigned pollset_increment = 0;
+
+/*
+ * The standard NuttX operation dispatch table can't call C++ member functions
+ * directly, so we have to bounce them through this dispatch table.
+ */
+static int	cdev_open(struct file *filp);
+static int	cdev_close(struct file *filp);
+static ssize_t	cdev_read(struct file *filp, char *buffer, size_t buflen);
+static ssize_t	cdev_write(struct file *filp, const char *buffer, size_t buflen);
+static off_t	cdev_seek(struct file *filp, off_t offset, int whence);
+static int	cdev_ioctl(struct file *filp, int cmd, unsigned long arg);
+static int	cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
+
+/**
+ * Character device indirection table.
+ *
+ * Every cdev we register gets the same function table; we use the private data
+ * field in the inode to store the instance pointer.
+ *
+ * Note that we use the GNU extension syntax here because we don't get designated
+ * initialisers in gcc 4.6.
+ */
+const struct file_operations CDev::fops = {
+open	: cdev_open,
+close	: cdev_close,
+read	: cdev_read,
+write	: cdev_write,
+seek	: cdev_seek,
+ioctl	: cdev_ioctl,
+poll	: cdev_poll,
+};
+
+CDev::CDev(const char *name,
+	   const char *devname,
+	   int irq) :
+	// base class
+	Device(name, irq),
+	// public
+	// protected
+	// private
+	_devname(devname),
+	_registered(false),
+	_open_count(0)
+{
+	for (unsigned i = 0; i < _max_pollwaiters; i++)
+		_pollset[i] = nullptr;
+}
+
+CDev::~CDev()
+{
+	if (_registered)
+		unregister_driver(_devname);
+}
+
+int
+CDev::init()
+{
+	int ret = OK;
+
+	// base class init first
+	ret = Device::init();
+
+	if (ret != OK)
+		goto out;
+
+	// now register the driver
+	ret = register_driver(_devname, &fops, 0666, (void *)this);
+
+	if (ret != OK)
+		goto out;
+
+	_registered = true;
+
+out:
+	return ret;
+}
+
+/*
+ * Default implementations of the character device interface
+ */
+int
+CDev::open(struct file *filp)
+{
+	int ret = OK;
+
+	lock();
+	/* increment the open count */
+	_open_count++;
+
+	if (_open_count == 1) {
+
+		/* first-open callback may decline the open */
+		ret = open_first(filp);
+
+		if (ret != OK)
+			_open_count--;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::open_first(struct file *filp)
+{
+	return OK;
+}
+
+int
+CDev::close(struct file *filp)
+{
+	int ret = OK;
+
+	lock();
+
+	if (_open_count > 0) {
+		/* decrement the open count */
+		_open_count--;
+
+		/* callback cannot decline the close */
+		if (_open_count == 0)
+			ret = close_last(filp);
+
+	} else {
+		ret = -EBADF;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::close_last(struct file *filp)
+{
+	return OK;
+}
+
+ssize_t
+CDev::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return -ENOSYS;
+}
+
+ssize_t
+CDev::write(struct file *filp, const char *buffer, size_t buflen)
+{
+	return -ENOSYS;
+}
+
+off_t
+CDev::seek(struct file *filp, off_t offset, int whence)
+{
+	return -ENOSYS;
+}
+
+int
+CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+		/* fetch a pointer to the driver's private data */
+	case DIOC_GETPRIV:
+		*(void **)(uintptr_t)arg = (void *)this;
+		return OK;
+	}
+
+	return -ENOTTY;
+}
+
+int
+CDev::poll(struct file *filp, struct pollfd *fds, bool setup)
+{
+	int ret = OK;
+
+	/*
+	 * Lock against pollnotify() (and possibly other callers)
+	 */
+	lock();
+
+	if (setup) {
+		/*
+		 * Save the file pointer in the pollfd for the subclass'
+		 * benefit.
+		 */
+		fds->priv = (void *)filp;
+
+		/*
+		 * Handle setup requests.
+		 */
+		ret = store_poll_waiter(fds);
+
+		if (ret == OK) {
+
+			/*
+			 * Check to see whether we should send a poll notification
+			 * immediately.
+			 */
+			fds->revents |= fds->events & poll_state(filp);
+
+			/* yes? post the notification */
+			if (fds->revents != 0)
+				sem_post(fds->sem);
+		}
+
+	} else {
+		/*
+		 * Handle a teardown request.
+		 */
+		ret = remove_poll_waiter(fds);
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+CDev::poll_notify(pollevent_t events)
+{
+	/* lock against poll() as well as other wakeups */
+	irqstate_t state = irqsave();
+
+	for (unsigned i = 0; i < _max_pollwaiters; i++)
+		if (nullptr != _pollset[i])
+			poll_notify_one(_pollset[i], events);
+
+	irqrestore(state);
+}
+
+void
+CDev::poll_notify_one(struct pollfd *fds, pollevent_t events)
+{
+	/* update the reported event set */
+	fds->revents |= fds->events & events;
+
+	/* if the state is now interesting, wake the waiter if it's still asleep */
+	/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
+	if ((fds->revents != 0) && (fds->sem->semcount <= 0))
+		sem_post(fds->sem);
+}
+
+pollevent_t
+CDev::poll_state(struct file *filp)
+{
+	/* by default, no poll events to report */
+	return 0;
+}
+
+int
+CDev::store_poll_waiter(struct pollfd *fds)
+{
+	/*
+	 * Look for a free slot.
+	 */
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (nullptr == _pollset[i]) {
+
+			/* save the pollfd */
+			_pollset[i] = fds;
+
+			return OK;
+		}
+	}
+
+	return ENOMEM;
+}
+
+int
+CDev::remove_poll_waiter(struct pollfd *fds)
+{
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (fds == _pollset[i]) {
+
+			_pollset[i] = nullptr;
+			return OK;
+
+		}
+	}
+
+	puts("poll: bad fd state");
+	return -EINVAL;
+}
+
+static int
+cdev_open(struct file *filp)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->open(filp);
+}
+
+static int
+cdev_close(struct file *filp)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->close(filp);
+}
+
+static ssize_t
+cdev_read(struct file *filp, char *buffer, size_t buflen)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->read(filp, buffer, buflen);
+}
+
+static ssize_t
+cdev_write(struct file *filp, const char *buffer, size_t buflen)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->write(filp, buffer, buflen);
+}
+
+static off_t
+cdev_seek(struct file *filp, off_t offset, int whence)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->seek(filp, offset, whence);
+}
+
+static int
+cdev_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->ioctl(filp, cmd, arg);
+}
+
+static int
+cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->poll(filp, fds, setup);
+}
+
+} // namespace device

src/drivers/device/device.cpp

@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 device.cpp
+ *
+ * Fundamental driver base class for the device framework.
+ */
+
+#include "device.h"
+
+#include <nuttx/arch.h>
+#include <stdio.h>
+#include <unistd.h>
+
+namespace device
+{
+
+/**
+ * Interrupt dispatch table entry.
+ */
+struct irq_entry {
+	int	irq;
+	Device	*owner;
+};
+
+static const unsigned	irq_nentries = 8;		/**< size of the interrupt dispatch table */
+static irq_entry	irq_entries[irq_nentries];	/**< interrupt dispatch table (XXX should be a vector) */
+
+/**
+ * Register an interrupt to a specific device.
+ *
+ * @param irq		The interrupt number to register.
+ * @param owner		The device receiving the interrupt.
+ * @return		OK if the interrupt was registered.
+ */
+static int	register_interrupt(int irq, Device *owner);
+
+/**
+ * Unregister an interrupt.
+ *
+ * @param irq		The previously-registered interrupt to be de-registered.
+ */
+static void	unregister_interrupt(int irq);
+
+/**
+ * Handle an interrupt.
+ *
+ * @param irq		The interrupt being invoked.
+ * @param context	The interrupt register context.
+ * @return		Always returns OK.
+ */
+static int	interrupt(int irq, void *context);
+
+Device::Device(const char *name,
+	       int irq) :
+	// public
+	// protected
+	_name(name),
+	_debug_enabled(false),
+	// private
+	_irq(irq),
+	_irq_attached(false)
+{
+	sem_init(&_lock, 0, 1);
+}
+
+Device::~Device()
+{
+	sem_destroy(&_lock);
+
+	if (_irq_attached)
+		unregister_interrupt(_irq);
+}
+
+int
+Device::init()
+{
+	int ret = OK;
+
+	// If assigned an interrupt, connect it
+	if (_irq) {
+		/* ensure it's disabled */
+		up_disable_irq(_irq);
+
+		/* register */
+		ret = register_interrupt(_irq, this);
+
+		if (ret != OK)
+			goto out;
+
+		_irq_attached = true;
+	}
+
+out:
+	return ret;
+}
+
+void
+Device::interrupt_enable()
+{
+	if (_irq_attached)
+		up_enable_irq(_irq);
+}
+
+void
+Device::interrupt_disable()
+{
+	if (_irq_attached)
+		up_disable_irq(_irq);
+}
+
+void
+Device::interrupt(void *context)
+{
+	// default action is to disable the interrupt so we don't get called again
+	interrupt_disable();
+}
+
+void
+Device::log(const char *fmt, ...)
+{
+	va_list	ap;
+
+	printf("[%s] ", _name);
+	va_start(ap, fmt);
+	vprintf(fmt, ap);
+	va_end(ap);
+	printf("\n");
+	fflush(stdout);
+}
+
+void
+Device::debug(const char *fmt, ...)
+{
+	va_list	ap;
+
+	if (_debug_enabled) {
+		printf("<%s> ", _name);
+		va_start(ap, fmt);
+		vprintf(fmt, ap);
+		va_end(ap);
+		printf("\n");
+		fflush(stdout);
+	}
+}
+
+static int
+register_interrupt(int irq, Device *owner)
+{
+	int ret = -ENOMEM;
+
+	// look for a slot where we can register the interrupt
+	for (unsigned i = 0; i < irq_nentries; i++) {
+		if (irq_entries[i].irq == 0) {
+
+			// great, we could put it here; try attaching it
+			ret = irq_attach(irq, &interrupt);
+
+			if (ret == OK) {
+				irq_entries[i].irq = irq;
+				irq_entries[i].owner = owner;
+			}
+
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static void
+unregister_interrupt(int irq)
+{
+	for (unsigned i = 0; i < irq_nentries; i++) {
+		if (irq_entries[i].irq == irq) {
+			irq_entries[i].irq = 0;
+			irq_entries[i].owner = nullptr;
+		}
+	}
+}
+
+static int
+interrupt(int irq, void *context)
+{
+	for (unsigned i = 0; i < irq_nentries; i++) {
+		if (irq_entries[i].irq == irq) {
+			irq_entries[i].owner->interrupt(context);
+			break;
+		}
+	}
+
+	return OK;
+}
+
+
+} // namespace device

src/drivers/device/device.h

@@ -0,0 +1,447 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 device.h
+ *
+ * Definitions for the generic base classes in the device framework.
+ */
+
+#ifndef _DEVICE_DEVICE_H
+#define _DEVICE_DEVICE_H
+
+/*
+ * Includes here should only cover the needs of the framework definitions.
+ */
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <poll.h>
+
+#include <nuttx/fs/fs.h>
+
+/**
+ * Namespace encapsulating all device framework classes, functions and data.
+ */
+namespace device __EXPORT
+{
+
+/**
+ * Fundamental base class for all device drivers.
+ *
+ * This class handles the basic "being a driver" things, including
+ * interrupt registration and dispatch.
+ */
+class __EXPORT Device
+{
+public:
+	/**
+	 * Interrupt handler.
+	 */
+	virtual void	interrupt(void *ctx);	/**< interrupt handler */
+
+protected:
+	const char	*_name;			/**< driver name */
+	bool		_debug_enabled;		/**< if true, debug messages are printed */
+
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param irq		Interrupt assigned to the device.
+	 */
+	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
+	 */
+	void		interrupt_enable();
+
+	/**
+	 * Disable the device interrupt
+	 */
+	void		interrupt_disable();
+
+	/**
+	 * Take the driver lock.
+	 *
+	 * Each driver instance has its own lock/semaphore.
+	 *
+	 * Note that we must loop as the wait may be interrupted by a signal.
+	 */
+	void		lock() {
+		do {} while (sem_wait(&_lock) != 0);
+	}
+
+	/**
+	 * Release the driver lock.
+	 */
+	void		unlock() {
+		sem_post(&_lock);
+	}
+
+	/**
+	 * Log a message.
+	 *
+	 * The message is prefixed with the driver name, and followed
+	 * by a newline.
+	 */
+	void		log(const char *fmt, ...);
+
+	/**
+	 * Print a debug message.
+	 *
+	 * The message is prefixed with the driver name, and followed
+	 * by a newline.
+	 */
+	void		debug(const char *fmt, ...);
+
+private:
+	int		_irq;
+	bool		_irq_attached;
+	sem_t		_lock;
+
+	/** disable copy construction for this and all subclasses */
+	Device(const Device &);
+
+	/** disable assignment for this and all subclasses */
+	Device &operator = (const Device &);
+
+	/**
+	 * Register ourselves as a handler for an interrupt
+	 *
+	 * @param irq		The interrupt to claim
+	 * @return		OK if the interrupt was registered
+	 */
+	int		dev_register_interrupt(int irq);
+
+	/**
+	 * Unregister ourselves as a handler for any interrupt
+	 */
+	void		dev_unregister_interrupt();
+
+	/**
+	 * Interrupt dispatcher
+	 *
+	 * @param irq		The interrupt that has been triggered.
+	 * @param context	Pointer to the interrupted context.
+	 */
+	static void	dev_interrupt(int irq, void *context);
+};
+
+/**
+ * Abstract class for any character device
+ */
+class __EXPORT CDev : public Device
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param irq		Interrupt assigned to the device
+	 */
+	CDev(const char *name, const char *devname, int irq = 0);
+
+	/**
+	 * Destructor
+	 */
+	~CDev();
+
+	virtual int	init();
+
+	/**
+	 * Handle an open of the device.
+	 *
+	 * This function is called for every open of the device. The default
+	 * implementation maintains _open_count and always returns OK.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the open is allowed, -errno otherwise.
+	 */
+	virtual int	open(struct file *filp);
+
+	/**
+	 * Handle a close of the device.
+	 *
+	 * This function is called for every close of the device. The default
+	 * implementation maintains _open_count and returns OK as long as it is not zero.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the close was successful, -errno otherwise.
+	 */
+	virtual int	close(struct file *filp);
+
+	/**
+	 * Perform a read from the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param buffer	Pointer to the buffer into which data should be placed.
+	 * @param buflen	The number of bytes to be read.
+	 * @return		The number of bytes read or -errno otherwise.
+	 */
+	virtual ssize_t	read(struct file *filp, char *buffer, size_t buflen);
+
+	/**
+	 * Perform a write to the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param buffer	Pointer to the buffer from which data should be read.
+	 * @param buflen	The number of bytes to be written.
+	 * @return		The number of bytes written or -errno otherwise.
+	 */
+	virtual ssize_t	write(struct file *filp, const char *buffer, size_t buflen);
+
+	/**
+	 * Perform a logical seek operation on the device.
+	 *
+	 * The default implementation returns -ENOSYS.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param offset	The new file position relative to whence.
+	 * @param whence	SEEK_OFS, SEEK_CUR or SEEK_END.
+	 * @return		The previous offset, or -errno otherwise.
+	 */
+	virtual off_t	seek(struct file *filp, off_t offset, int whence);
+
+	/**
+	 * Perform an ioctl operation on the device.
+	 *
+	 * The default implementation handles DIOC_GETPRIV, and otherwise
+	 * returns -ENOTTY. Subclasses should call the default implementation
+	 * for any command they do not handle themselves.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param cmd		The ioctl command value.
+	 * @param arg		The ioctl argument value.
+	 * @return		OK on success, or -errno otherwise.
+	 */
+	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Perform a poll setup/teardown operation.
+	 *
+	 * This is handled internally and should not normally be overridden.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @param fds		Poll descriptor being waited on.
+	 * @param arg		True if this is establishing a request, false if
+	 *			it is being torn down.
+	 * @return		OK on success, or -errno otherwise.
+	 */
+	virtual int	poll(struct file *filp, struct pollfd *fds, bool setup);
+
+	/**
+	 * Test whether the device is currently open.
+	 *
+	 * This can be used to avoid tearing down a device that is still active.
+	 *
+	 * @return              True if the device is currently open.
+	 */
+	bool            is_open() { return _open_count > 0; }
+
+protected:
+	/**
+	 * Pointer to the default cdev file operations table; useful for
+	 * registering clone devices etc.
+	 */
+	static const struct file_operations	fops;
+
+	/**
+	 * Check the current state of the device for poll events from the
+	 * perspective of the file.
+	 *
+	 * This function is called by the default poll() implementation when
+	 * a poll is set up to determine whether the poll should return immediately.
+	 *
+	 * The default implementation returns no events.
+	 *
+	 * @param filp		The file that's interested.
+	 * @return		The current set of poll events.
+	 */
+	virtual pollevent_t poll_state(struct file *filp);
+
+	/**
+	 * Report new poll events.
+	 *
+	 * This function should be called anytime the state of the device changes
+	 * in a fashion that might be interesting to a poll waiter.
+	 *
+	 * @param events	The new event(s) being announced.
+	 */
+	virtual void	poll_notify(pollevent_t events);
+
+	/**
+	 * Internal implementation of poll_notify.
+	 *
+	 * @param fds		A poll waiter to notify.
+	 * @param events	The event(s) to send to the waiter.
+	 */
+	virtual void	poll_notify_one(struct pollfd *fds, pollevent_t events);
+
+	/**
+	 * Notification of the first open.
+	 *
+	 * This function is called when the device open count transitions from zero
+	 * to one.  The driver lock is held for the duration of the call.
+	 *
+	 * The default implementation returns OK.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the open should proceed, -errno otherwise.
+	 */
+	virtual int	open_first(struct file *filp);
+
+	/**
+	 * Notification of the last close.
+	 *
+	 * This function is called when the device open count transitions from
+	 * one to zero.  The driver lock is held for the duration of the call.
+	 *
+	 * The default implementation returns OK.
+	 *
+	 * @param filp		Pointer to the NuttX file structure.
+	 * @return		OK if the open should return OK, -errno otherwise.
+	 */
+	virtual int	close_last(struct file *filp);
+
+private:
+	static const unsigned _max_pollwaiters = 8;
+
+	const char	*_devname;		/**< device node name */
+	bool		_registered;		/**< true if device name was registered */
+	unsigned	_open_count;		/**< number of successful opens */
+
+	struct pollfd	*_pollset[_max_pollwaiters];
+
+	/**
+	 * Store a pollwaiter in a slot where we can find it later.
+	 *
+	 * Expands the pollset as required.  Must be called with the driver locked.
+	 *
+	 * @return		OK, or -errno on error.
+	 */
+	int		store_poll_waiter(struct pollfd *fds);
+
+	/**
+	 * Remove a poll waiter.
+	 *
+	 * @return		OK, or -errno on error.
+	 */
+	int		remove_poll_waiter(struct pollfd *fds);
+};
+
+/**
+ * Abstract class for character device accessed via PIO
+ */
+class __EXPORT PIO : public CDev
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param base		Base address of the device PIO area
+	 * @param irq		Interrupt assigned to the device (or zero if none)
+	 */
+	PIO(const char *name,
+	    const char *devname,
+	    uint32_t base,
+	    int irq = 0);
+	~PIO();
+
+	int		init();
+
+protected:
+
+	/**
+	 * Read a register
+	 *
+	 * @param offset	Register offset in bytes from the base address.
+	 */
+	uint32_t reg(uint32_t offset) {
+		return *(volatile uint32_t *)(_base + offset);
+	}
+
+	/**
+	 * Write a register
+	 *
+	 * @param offset	Register offset in bytes from the base address.
+	 * @param value	Value to write.
+	 */
+	void		reg(uint32_t offset, uint32_t value) {
+		*(volatile uint32_t *)(_base + offset) = value;
+	}
+
+	/**
+	 * Modify a register
+	 *
+	 * Note that there is a risk of a race during the read/modify/write cycle
+	 * that must be taken care of by the caller.
+	 *
+	 * @param offset	Register offset in bytes from the base address.
+	 * @param clearbits	Bits to clear in the register
+	 * @param setbits	Bits to set in the register
+	 */
+	void		modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) {
+		uint32_t val = reg(offset);
+		val &= ~clearbits;
+		val |= setbits;
+		reg(offset, val);
+	}
+
+private:
+	uint32_t	_base;
+};
+
+} // namespace device
+
+#endif /* _DEVICE_DEVICE_H */

src/drivers/device/i2c.cpp

@@ -0,0 +1,204 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 i2c.cpp
+ *
+ * Base class for devices attached via the I2C bus.
+ *
+ * @todo Bus frequency changes; currently we do nothing with the value
+ *       that is supplied.  Should we just depend on the bus knowing?
+ */
+
+#include "i2c.h"
+
+namespace device
+{
+
+I2C::I2C(const char *name,
+	 const char *devname,
+	 int bus,
+	 uint16_t address,
+	 uint32_t frequency,
+	 int irq) :
+	// base class
+	CDev(name, devname, irq),
+	// public
+	// protected
+	_retries(0),
+	// private
+	_bus(bus),
+	_address(address),
+	_frequency(frequency),
+	_dev(nullptr)
+{
+}
+
+I2C::~I2C()
+{
+	if (_dev)
+		up_i2cuninitialize(_dev);
+}
+
+int
+I2C::init()
+{
+	int ret = OK;
+
+	/* attach to the i2c bus */
+	_dev = up_i2cinitialize(_bus);
+
+	if (_dev == nullptr) {
+		debug("failed to init I2C");
+		ret = -ENOENT;
+		goto out;
+	}
+
+	// call the probe function to check whether the device is present
+	ret = probe();
+
+	if (ret != OK) {
+		debug("probe failed");
+		goto out;
+	}
+
+	// do base class init, which will create device node, etc
+	ret = CDev::init();
+
+	if (ret != OK) {
+		debug("cdev init failed");
+		goto out;
+	}
+
+	// tell the world where we are
+	log("on I2C bus %d at 0x%02x", _bus, _address);
+
+out:
+	return ret;
+}
+
+int
+I2C::probe()
+{
+	// Assume the device is too stupid to be discoverable.
+	return OK;
+}
+
+int
+I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	struct i2c_msg_s msgv[2];
+	unsigned msgs;
+	int ret;
+	unsigned retry_count = 0;
+
+	do {
+		//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+
+		msgs = 0;
+
+		if (send_len > 0) {
+			msgv[msgs].addr = _address;
+			msgv[msgs].flags = 0;
+			msgv[msgs].buffer = const_cast<uint8_t *>(send);
+			msgv[msgs].length = send_len;
+			msgs++;
+		}
+
+		if (recv_len > 0) {
+			msgv[msgs].addr = _address;
+			msgv[msgs].flags = I2C_M_READ;
+			msgv[msgs].buffer = recv;
+			msgv[msgs].length = recv_len;
+			msgs++;
+		}
+
+		if (msgs == 0)
+			return -EINVAL;
+
+		/*
+		 * I2C architecture means there is an unavoidable race here
+		 * if there are any devices on the bus with a different frequency
+		 * preference.  Really, this is pointless.
+		 */
+		I2C_SETFREQUENCY(_dev, _frequency);
+		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
+
+		/* success */
+		if (ret == OK)
+			break;
+
+		/* if we have already retried once, or we are going to give up, then reset the bus */
+		if ((retry_count >= 1) || (retry_count >= _retries))
+			up_i2creset(_dev);
+
+	} while (retry_count++ < _retries);
+
+	return ret;
+
+}
+
+int
+I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
+{
+	int ret;
+	unsigned retry_count = 0;
+
+	/* force the device address into the message vector */
+	for (unsigned i = 0; i < msgs; i++)
+		msgv[i].addr = _address;
+
+
+	do {
+		/*
+		 * I2C architecture means there is an unavoidable race here
+		 * if there are any devices on the bus with a different frequency
+		 * preference.  Really, this is pointless.
+		 */
+		I2C_SETFREQUENCY(_dev, _frequency);
+		ret = I2C_TRANSFER(_dev, msgv, msgs);
+
+		/* success */
+		if (ret == OK)
+			break;
+
+		/* if we have already retried once, or we are going to give up, then reset the bus */
+		if ((retry_count >= 1) || (retry_count >= _retries))
+			up_i2creset(_dev);
+
+	} while (retry_count++ < _retries);
+
+	return ret;
+}
+
+} // namespace device

src/drivers/device/i2c.h

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 i2c.h
+ *
+ * Base class for devices connected via I2C.
+ */
+
+#ifndef _DEVICE_I2C_H
+#define _DEVICE_I2C_H
+
+#include "device.h"
+
+#include <nuttx/i2c.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on I2C
+ */
+class __EXPORT I2C : public CDev
+{
+
+protected:
+	/**
+	 * The number of times a read or write operation will be retried on
+	 * error.
+	 */
+	unsigned		_retries;
+
+	/**
+	 * The I2C bus number the device is attached to.
+	 */
+	int			_bus;
+
+	/**
+	 * @ Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param bus		I2C bus on which the device lives
+	 * @param address	I2C bus address, or zero if set_address will be used
+	 * @param frequency	I2C bus frequency for the device (currently not used)
+	 * @param irq		Interrupt assigned to the device (or zero if none)
+	 */
+	I2C(const char *name,
+	    const char *devname,
+	    int bus,
+	    uint16_t address,
+	    uint32_t frequency,
+	    int irq = 0);
+	~I2C();
+
+	virtual int	init();
+
+	/**
+	 * Check for the presence of the device on the bus.
+	 */
+	virtual int	probe();
+
+	/**
+	 * Perform an I2C transaction to the device.
+	 *
+	 * At least one of send_len and recv_len must be non-zero.
+	 *
+	 * @param send		Pointer to bytes to send.
+	 * @param send_len	Number of bytes to send.
+	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv_len	Number of bytes to receive.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(const uint8_t *send, unsigned send_len,
+				 uint8_t *recv, unsigned recv_len);
+
+	/**
+	 * Perform a multi-part I2C transaction to the device.
+	 *
+	 * @param msgv		An I2C message vector.
+	 * @param msgs		The number of entries in the message vector.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(i2c_msg_s *msgv, unsigned msgs);
+
+	/**
+	 * Change the bus address.
+	 *
+	 * Most often useful during probe() when the driver is testing
+	 * several possible bus addresses.
+	 *
+	 * @param address	The new bus address to set.
+	 */
+	void		set_address(uint16_t address) {
+		_address = address;
+	}
+
+private:
+	uint16_t		_address;
+	uint32_t		_frequency;
+	struct i2c_dev_s	*_dev;
+};
+
+} // namespace device
+
+#endif /* _DEVICE_I2C_H */

src/drivers/device/module.mk

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# Build the device driver framework.
+#
+
+SRCS		= cdev.cpp \
+		  device.cpp \
+		  i2c.cpp \
+		  pio.cpp \
+		  spi.cpp

src/drivers/device/pio.cpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 pio.cpp
+ *
+ * Base class for devices accessed via PIO to registers.
+ */
+
+#include "device.h"
+
+namespace device
+{
+
+PIO::PIO(const char *name,
+	 const char *devname,
+	 uint32_t base,
+	 int irq) :
+	// base class
+	CDev(name, devname, irq),
+	// public
+	// protected
+	// private
+	_base(base)
+{
+}
+
+PIO::~PIO()
+{
+	// nothing to do here...
+}
+
+int
+PIO::init()
+{
+	int ret = OK;
+
+	// base class init first
+	ret = CDev::init();
+
+	return ret;
+}
+
+} // namespace device

src/drivers/device/spi.cpp

@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 spi.cpp
+ *
+ * Base class for devices connected via SPI.
+ *
+ * @todo Work out if caching the mode/frequency would save any time.
+ *
+ * @todo A separate bus/device abstraction would allow for mixed interrupt-mode
+ * and non-interrupt-mode clients to arbitrate for the bus.  As things stand,
+ * a bus shared between clients of both kinds is vulnerable to races between
+ * the two, where an interrupt-mode client will ignore the lock held by the
+ * non-interrupt-mode client.
+ */
+
+#include <nuttx/arch.h>
+
+#include "spi.h"
+
+#ifndef CONFIG_SPI_EXCHANGE
+# error This driver requires CONFIG_SPI_EXCHANGE
+#endif
+
+namespace device
+{
+
+SPI::SPI(const char *name,
+	 const char *devname,
+	 int bus,
+	 enum spi_dev_e device,
+	 enum spi_mode_e mode,
+	 uint32_t frequency,
+	 int irq) :
+	// base class
+	CDev(name, devname, irq),
+	// public
+	// protected
+	// private
+	_bus(bus),
+	_device(device),
+	_mode(mode),
+	_frequency(frequency),
+	_dev(nullptr)
+{
+}
+
+SPI::~SPI()
+{
+	// XXX no way to let go of the bus...
+}
+
+int
+SPI::init()
+{
+	int ret = OK;
+
+	/* attach to the spi bus */
+	if (_dev == nullptr)
+		_dev = up_spiinitialize(_bus);
+
+	if (_dev == nullptr) {
+		debug("failed to init SPI");
+		ret = -ENOENT;
+		goto out;
+	}
+
+	/* deselect device to ensure high to low transition of pin select */
+	SPI_SELECT(_dev, _device, false);
+
+	/* call the probe function to check whether the device is present */
+	ret = probe();
+
+	if (ret != OK) {
+		debug("probe failed");
+		goto out;
+	}
+
+	/* do base class init, which will create the device node, etc. */
+	ret = CDev::init();
+
+	if (ret != OK) {
+		debug("cdev init failed");
+		goto out;
+	}
+
+	/* tell the workd where we are */
+	log("on SPI bus %d at %d", _bus, _device);
+
+out:
+	return ret;
+}
+
+int
+SPI::probe()
+{
+	// assume the device is too stupid to be discoverable
+	return OK;
+}
+
+int
+SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
+{
+
+	if ((send == nullptr) && (recv == nullptr))
+		return -EINVAL;
+
+	/* do common setup */
+	if (!up_interrupt_context())
+		SPI_LOCK(_dev, true);
+
+	SPI_SETFREQUENCY(_dev, _frequency);
+	SPI_SETMODE(_dev, _mode);
+	SPI_SETBITS(_dev, 8);
+	SPI_SELECT(_dev, _device, true);
+
+	/* do the transfer */
+	SPI_EXCHANGE(_dev, send, recv, len);
+
+	/* and clean up */
+	SPI_SELECT(_dev, _device, false);
+
+	if (!up_interrupt_context())
+		SPI_LOCK(_dev, false);
+
+	return OK;
+}
+
+} // namespace device

src/drivers/device/spi.h

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 spi.h
+ *
+ * Base class for devices connected via SPI.
+ */
+
+#ifndef _DEVICE_SPI_H
+#define _DEVICE_SPI_H
+
+#include "device.h"
+
+#include <nuttx/spi.h>
+
+namespace device __EXPORT
+{
+
+/**
+ * Abstract class for character device on SPI
+ */
+class __EXPORT SPI : public CDev
+{
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param name		Driver name
+	 * @param devname	Device node name
+	 * @param bus		SPI bus on which the device lives
+	 * @param device	Device handle (used by SPI_SELECT)
+	 * @param mode		SPI clock/data mode
+	 * @param frequency	SPI clock frequency
+	 * @param irq		Interrupt assigned to the device (or zero if none)
+	 */
+	SPI(const char *name,
+	    const char *devname,
+	    int bus,
+	    enum spi_dev_e device,
+	    enum spi_mode_e mode,
+	    uint32_t frequency,
+	    int irq = 0);
+	~SPI();
+
+	virtual int	init();
+
+	/**
+	 * Check for the presence of the device on the bus.
+	 */
+	virtual int	probe();
+
+	/**
+	 * Perform a SPI transfer.
+	 *
+	 * If called from interrupt context, this interface does not lock
+	 * the bus and may interfere with non-interrupt-context callers.
+	 *
+	 * Clients in a mixed interrupt/non-interrupt configuration must
+	 * ensure appropriate interlocking.
+	 *
+	 * At least one of send or recv must be non-null.
+	 *
+	 * @param send		Bytes to send to the device, or nullptr if
+	 *			no data is to be sent.
+	 * @param recv		Buffer for receiving bytes from the device,
+	 *			or nullptr if no bytes are to be received.
+	 * @param len		Number of bytes to transfer.
+	 * @return		OK if the exchange was successful, -errno
+	 *			otherwise.
+	 */
+	int		transfer(uint8_t *send, uint8_t *recv, unsigned len);
+
+private:
+	int			_bus;
+	enum spi_dev_e		_device;
+	enum spi_mode_e		_mode;
+	uint32_t		_frequency;
+	struct spi_dev_s	*_dev;
+};
+
+} // namespace device
+
+#endif /* _DEVICE_SPI_H */

src/drivers/drv_accel.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Accelerometer driver interface.
+ */
+
+#ifndef _DRV_ACCEL_H
+#define _DRV_ACCEL_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define ACCEL_DEVICE_PATH	"/dev/accel"
+
+/**
+ * accel report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct accel_report {
+	uint64_t timestamp;
+	float x;		/**< acceleration in the NED X board axis in m/s^2 */
+	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
+	float z;		/**< acceleration in the NED Z board axis in m/s^2 */
+	float temperature;	/**< temperature in degrees celsius */
+	float range_m_s2;	/**< range in m/s^2 (+- this value) */
+	float scaling;
+
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
+	int16_t temperature_raw;
+};
+
+/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct accel_scale {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
+
+/*
+ * ObjDev tag for raw accelerometer data.
+ */
+ORB_DECLARE(sensor_accel);
+
+/*
+ * ioctl() definitions
+ *
+ * Accelerometer drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _ACCELIOCBASE		(0x2100)
+#define _ACCELIOC(_n)		(_IOC(_ACCELIOCBASE, _n))
+
+
+/** set the accel internal sample rate to at least (arg) Hz */
+#define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
+
+/** return the accel internal sample rate in Hz */
+#define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
+
+/** set the accel internal lowpass filter to no lower than (arg) Hz */
+#define ACCELIOCSLOWPASS	_ACCELIOC(2)
+
+/** return the accel internal lowpass filter in Hz */
+#define ACCELIOCGLOWPASS	_ACCELIOC(3)
+
+/** set the accel scaling constants to the structure pointed to by (arg) */
+#define ACCELIOCSSCALE		_ACCELIOC(5)
+
+/** get the accel scaling constants into the structure pointed to by (arg) */
+#define ACCELIOCGSCALE		_ACCELIOC(6)
+
+/** set the accel measurement range to handle at least (arg) g */
+#define ACCELIOCSRANGE		_ACCELIOC(7)
+
+/** get the current accel measurement range in g */
+#define ACCELIOCGRANGE		_ACCELIOC(8)
+
+/** get the result of a sensor self-test */
+#define ACCELIOCSELFTEST	_ACCELIOC(9)
+
+#endif /* _DRV_ACCEL_H */

src/drivers/drv_adc.h

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_adc.h
+ *
+ * ADC driver interface.
+ *
+ * This defines additional operations over and above the standard NuttX
+ * ADC API.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define ADC_DEVICE_PATH		"/dev/adc0"
+
+/*
+ * ioctl definitions
+ */

src/drivers/drv_baro.h

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Barometric pressure sensor driver interface.
+ */
+
+#ifndef _DRV_BARO_H
+#define _DRV_BARO_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define BARO_DEVICE_PATH	"/dev/baro"
+
+/**
+ * baro report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct baro_report {
+	float pressure;
+	float altitude;
+	float temperature;
+	uint64_t timestamp;
+};
+
+/*
+ * ObjDev tag for raw barometer data.
+ */
+ORB_DECLARE(sensor_baro);
+
+/*
+ * ioctl() definitions
+ */
+
+#define _BAROIOCBASE		(0x2200)
+#define _BAROIOC(_n)		(_IOC(_BAROIOCBASE, _n))
+
+/** set corrected MSL pressure in pascals */
+#define BAROIOCSMSLPRESSURE	_BAROIOC(0)
+
+/** get current MSL pressure in pascals */
+#define BAROIOCGMSLPRESSURE	_BAROIOC(1)
+
+#endif /* _DRV_BARO_H */

src/drivers/drv_blinkm.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_blinkm.h
+ *
+ * BlinkM driver API
+ *
+ * This could probably become a more generalised API for multi-colour LED
+ * driver systems, or be merged with the generic LED driver.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define BLINKM_DEVICE_PATH	"/dev/blinkm"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _BLINKMIOCBASE		(0x2900)
+#define _BLINKMIOC(_n)		(_IOC(_BLINKMIOCBASE, _n))
+
+/** play the named script in *(char *)arg, repeating forever */
+#define BLINKM_PLAY_SCRIPT_NAMED	_BLINKMIOC(1)
+
+/** play the numbered script in (arg), repeating forever */
+#define BLINKM_PLAY_SCRIPT		_BLINKMIOC(2)
+
+/** 
+ * Set the user script; (arg) is a pointer to an array of script lines,
+ * where each line is an array of four bytes giving <duration>, <command>, arg[0-2]
+ *
+ * The script is terminated by a zero command.
+ */
+#define BLINKM_SET_USER_SCRIPT		_BLINKMIOC(3)

src/drivers/drv_gpio.h

@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_gpio.h
+ *
+ * Generic GPIO ioctl interface.
+ */
+
+#ifndef _DRV_GPIO_H
+#define _DRV_GPIO_H
+
+#include <sys/ioctl.h>
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU
+/*
+ * PX4FMU GPIO numbers.
+ *
+ * For shared pins, alternate function 1 selects the non-GPIO mode 
+ * (USART2, CAN2, etc.)
+ */
+# define GPIO_EXT_1		(1<<0)		/**< high-power GPIO 1 */
+# define GPIO_EXT_2		(1<<1)		/**< high-power GPIO 1 */
+# define GPIO_MULTI_1		(1<<2)		/**< USART2 CTS */
+# define GPIO_MULTI_2		(1<<3)		/**< USART2 RTS */
+# define GPIO_MULTI_3		(1<<4)		/**< USART2 TX */
+# define GPIO_MULTI_4		(1<<5)		/**< USART2 RX */
+# define GPIO_CAN_TX		(1<<6)		/**< CAN2 TX */
+# define GPIO_CAN_RX		(1<<7)		/**< CAN2 RX */
+
+/**
+ * Default GPIO device - other devices may also support this protocol if
+ * they also export GPIO-like things.  This is always the GPIOs on the
+ * main board.
+ */
+# define GPIO_DEVICE_PATH	"/dev/px4fmu"
+
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMUV2
+/*
+ * PX4FMUv2 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1		(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2		(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3		(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4		(1<<3)		/**< servo 4 output */
+# define GPIO_SERVO_5		(1<<4)		/**< servo 5 output */
+# define GPIO_SERVO_6		(1<<5)		/**< servo 6 output */
+
+# define GPIO_5V_PERIPH_EN	(1<<6)		/**< PA8 - VDD_5V_PERIPH_EN */
+# define GPIO_5V_HIPOWER_OC	(1<<7)		/**< PE10 - !VDD_5V_HIPOWER_OC */
+# define GPIO_5V_PERIPH_OC	(1<<8)		/**< PE15 - !VDD_5V_PERIPH_OC */
+
+/**
+ * Default GPIO device - other devices may also support this protocol if
+ * they also export GPIO-like things.  This is always the GPIOs on the
+ * main board.
+ */
+# define GPIO_DEVICE_PATH	"/dev/px4fmu"
+
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO
+/*
+ * PX4IO GPIO numbers.
+ *
+ * XXX note that these are here for reference/future use; currently
+ * there is no good way to wire these up without a common STM32 GPIO
+ * driver, which isn't implemented yet.
+ */
+/* outputs */
+# define GPIO_ACC1_POWER	(1<<0)		/**< accessory power 1 */
+# define GPIO_ACC2_POWER	(1<<1)		/**< accessory power 2 */
+# define GPIO_SERVO_POWER	(1<<2)		/**< servo power */
+# define GPIO_RELAY1		(1<<3)		/**< relay 1 */
+# define GPIO_RELAY2		(1<<4)		/**< relay 2 */
+# define GPIO_LED_BLUE		(1<<5)		/**< blue LED */
+# define GPIO_LED_AMBER		(1<<6)		/**< amber/red LED */
+# define GPIO_LED_SAFETY	(1<<7)		/**< safety LED */
+
+/* inputs */
+# define GPIO_ACC_OVERCURRENT	(1<<8)		/**< accessory 1/2 overcurrent detect */
+# define GPIO_SERVO_OVERCURRENT	(1<<9)		/**< servo overcurrent detect */
+# define GPIO_SAFETY_BUTTON	(1<<10)		/**< safety button pressed */
+
+/**
+ * Default GPIO device - other devices may also support this protocol if
+ * they also export GPIO-like things.  This is always the GPIOs on the
+ * main board.
+ */
+# define GPIO_DEVICE_PATH	"/dev/px4io"
+
+#endif
+
+#ifndef GPIO_DEVICE_PATH
+#  error No GPIO support for this board.
+#endif
+
+/*
+ * IOCTL definitions.
+ *
+ * For all ioctls, the (arg) argument is a bitmask of GPIOs to be affected
+ * by the operation, with the LSB being the lowest-numbered GPIO.
+ *
+ * Note that there may be board-specific relationships between GPIOs;
+ * applications using GPIOs should be aware of this.
+ */
+#define _GPIOCBASE	0x2700
+#define GPIOC(_x)	_IOC(_GPIOCBASE, _x)
+
+/** reset all board GPIOs to their default state */
+#define GPIO_RESET	GPIOC(0)
+
+/** configure the board GPIOs in (arg) as outputs */
+#define GPIO_SET_OUTPUT	GPIOC(1)
+
+/** configure the board GPIOs in (arg) as inputs */
+#define GPIO_SET_INPUT	GPIOC(2)
+
+/** configure the board GPIOs in (arg) for the first alternate function (if supported) */
+#define GPIO_SET_ALT_1	GPIOC(3)
+
+/** configure the board GPIO (arg) for the second alternate function (if supported) */
+#define GPIO_SET_ALT_2	GPIOC(4)
+
+/** configure the board GPIO (arg) for the third alternate function (if supported) */
+#define GPIO_SET_ALT_3	GPIOC(5)
+
+/** configure the board GPIO (arg) for the fourth alternate function (if supported) */
+#define GPIO_SET_ALT_4	GPIOC(6)
+
+/** set the GPIOs in (arg) */
+#define GPIO_SET	GPIOC(10)
+
+/** clear the GPIOs in (arg) */
+#define GPIO_CLEAR	GPIOC(11)
+
+/** read all the GPIOs and return their values in *(uint32_t *)arg */
+#define GPIO_GET	GPIOC(12)
+
+#endif /* _DRV_GPIO_H */

src/drivers/drv_gps.h

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 GPS driver interface.
+ */
+
+#ifndef _DRV_GPS_H
+#define _DRV_GPS_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+
+#define GPS_DEVICE_PATH	"/dev/gps"
+
+typedef enum {
+	GPS_DRIVER_MODE_NONE = 0,
+	GPS_DRIVER_MODE_UBX,
+	GPS_DRIVER_MODE_MTK,
+	GPS_DRIVER_MODE_NMEA,
+} gps_driver_mode_t;
+
+
+/*
+ * ObjDev tag for GPS data.
+ */
+ORB_DECLARE(gps);
+
+/*
+ * ioctl() definitions
+ */
+#define _GPSIOCBASE			(0x2800)            //TODO: arbitrary choice...
+#define _GPSIOC(_n)		(_IOC(_GPSIOCBASE, _n))
+
+#endif /* _DRV_GPS_H */

src/drivers/drv_gyro.h

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Gyroscope driver interface.
+ */
+
+#ifndef _DRV_GYRO_H
+#define _DRV_GYRO_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define GYRO_DEVICE_PATH	"/dev/gyro"
+
+/**
+ * gyro report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct gyro_report {
+	uint64_t timestamp;
+	float x;		/**< angular velocity in the NED X board axis in rad/s */
+	float y;		/**< angular velocity in the NED Y board axis in rad/s */
+	float z;		/**< angular velocity in the NED Z board axis in rad/s */
+	float temperature;	/**< temperature in degrees celcius */
+	float range_rad_s;
+	float scaling;
+
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
+	int16_t temperature_raw;
+};
+
+/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct gyro_scale {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
+
+/*
+ * ObjDev tag for raw gyro data.
+ */
+ORB_DECLARE(sensor_gyro);
+
+/*
+ * ioctl() definitions
+ */
+
+#define _GYROIOCBASE		(0x2300)
+#define _GYROIOC(_n)		(_IOC(_GYROIOCBASE, _n))
+
+/** set the gyro internal sample rate to at least (arg) Hz */
+#define GYROIOCSSAMPLERATE	_GYROIOC(0)
+
+/** return the gyro internal sample rate in Hz */
+#define GYROIOCGSAMPLERATE	_GYROIOC(1)
+
+/** set the gyro internal lowpass filter to no lower than (arg) Hz */
+#define GYROIOCSLOWPASS		_GYROIOC(2)
+
+/** set the gyro internal lowpass filter to no lower than (arg) Hz */
+#define GYROIOCGLOWPASS		_GYROIOC(3)
+
+/** set the gyro scaling constants to (arg) */
+#define GYROIOCSSCALE		_GYROIOC(4)
+
+/** get the gyro scaling constants into (arg) */
+#define GYROIOCGSCALE		_GYROIOC(5)
+
+/** set the gyro measurement range to handle at least (arg) degrees per second */
+#define GYROIOCSRANGE		_GYROIOC(6)
+
+/** get the current gyro measurement range in degrees per second */
+#define GYROIOCGRANGE		_GYROIOC(7)
+
+/** check the status of the sensor */
+#define GYROIOCSELFTEST		_GYROIOC(8)
+
+#endif /* _DRV_GYRO_H */

src/drivers/drv_hrt.h

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.h
+ *
+ * High-resolution timer with callouts and timekeeping.
+ */
+
+#pragma once
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <time.h>
+#include <queue.h>
+
+__BEGIN_DECLS
+
+/*
+ * Absolute time, in microsecond units.
+ *
+ * Absolute time is measured from some arbitrary epoch shortly after
+ * system startup.  It should never wrap or go backwards.
+ */
+typedef uint64_t	hrt_abstime;
+
+/*
+ * Callout function type.
+ *
+ * Note that callouts run in the timer interrupt context, so
+ * they are serialised with respect to each other, and must not
+ * block.
+ */
+typedef void	(* hrt_callout)(void *arg);
+
+/*
+ * Callout record.
+ */
+typedef struct hrt_call {
+	struct sq_entry_s	link;
+
+	hrt_abstime		deadline;
+	hrt_abstime		period;
+	hrt_callout		callout;
+	void			*arg;
+} *hrt_call_t;
+
+/*
+ * Get absolute time.
+ */
+__EXPORT extern hrt_abstime hrt_absolute_time(void);
+
+/*
+ * Convert a timespec to absolute time.
+ */
+__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
+
+/*
+ * Convert absolute time to a timespec.
+ */
+__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+
+/*
+ * 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.
+ */
+__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
+
+/*
+ * Store the absolute time in an interrupt-safe fashion.
+ *
+ * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
+ */
+__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+__EXPORT extern void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+__EXPORT extern void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *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.
+ */
+__EXPORT extern void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
+
+/*
+ * 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.
+ */
+__EXPORT extern bool	hrt_called(struct hrt_call *entry);
+
+/*
+ * Remove the entry from the callout list.
+ */
+__EXPORT extern void	hrt_cancel(struct hrt_call *entry);
+
+/*
+ * Initialise the HRT.
+ */
+__EXPORT extern void	hrt_init(void);
+
+__END_DECLS

src/drivers/drv_led.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_led.h
+ *
+ * LED driver API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define LED_DEVICE_PATH		"/dev/led"
+
+#define _LED_BASE		0x2800
+
+/* PX4 LED colour codes */
+#define LED_AMBER		1
+#define LED_RED			1	/* some boards have red rather than amber */
+#define LED_BLUE		0
+#define LED_SAFETY		2
+
+#define LED_ON			_IOC(_LED_BASE, 0)
+#define LED_OFF			_IOC(_LED_BASE, 1)
+
+__BEGIN_DECLS
+
+/*
+ * Initialise the LED driver.
+ */
+__EXPORT extern void drv_led_start(void);
+
+__END_DECLS

src/drivers/drv_mag.h

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Magnetometer driver interface.
+ */
+
+#ifndef _DRV_MAG_H
+#define _DRV_MAG_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define MAG_DEVICE_PATH		"/dev/mag"
+
+/**
+ * mag report structure.  Reads from the device must be in multiples of this
+ * structure.
+ *
+ * Output values are in gauss.
+ */
+struct mag_report {
+	uint64_t timestamp;
+	float x;
+	float y;
+	float z;
+	float range_ga;
+	float scaling;
+
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
+};
+
+/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct mag_scale {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
+
+/*
+ * ObjDev tag for raw magnetometer data.
+ */
+ORB_DECLARE(sensor_mag);
+
+/*
+ * ioctl() definitions
+ */
+
+#define _MAGIOCBASE		(0x2400)
+#define _MAGIOC(_n)		(_IOC(_MAGIOCBASE, _n))
+
+/** set the mag internal sample rate to at least (arg) Hz */
+#define MAGIOCSSAMPLERATE	_MAGIOC(0)
+
+/** set the mag internal lowpass filter to no lower than (arg) Hz */
+#define MAGIOCSLOWPASS		_MAGIOC(1)
+
+/** set the mag scaling constants to the structure pointed to by (arg) */
+#define MAGIOCSSCALE		_MAGIOC(2)
+
+/** copy the mag scaling constants to the structure pointed to by (arg) */
+#define MAGIOCGSCALE		_MAGIOC(3)
+
+/** set the measurement range to handle (at least) arg Gauss */
+#define MAGIOCSRANGE		_MAGIOC(4)
+
+/** perform self-calibration, update scale factors to canonical units */
+#define MAGIOCCALIBRATE		_MAGIOC(5)
+
+/** excite strap */
+#define MAGIOCEXSTRAP		_MAGIOC(6)
+
+/** perform self test and report status */
+#define MAGIOCSELFTEST		_MAGIOC(7)
+
+#endif /* _DRV_MAG_H */

src/drivers/drv_mixer.h

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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_mixer.h
+ *
+ * Mixer ioctl interfaces.
+ *
+ * Normal workflow is:
+ *
+ * - open mixer device
+ * - add mixer(s)
+ * loop:
+ *  - mix actuators to array
+ *
+ * Each client has its own configuration.
+ *
+ * When mixing, outputs are produced by mixers in the order they are
+ * added.  A simple mixer produces one output; a multotor mixer will
+ * produce several outputs, etc.
+ */
+
+#ifndef _DRV_MIXER_H
+#define _DRV_MIXER_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define MIXER_DEVICE_PATH		"/dev/mixer"
+
+/*
+ * ioctl() definitions
+ */
+#define _MIXERIOCBASE		(0x2500)
+#define _MIXERIOC(_n)		(_IOC(_MIXERIOCBASE, _n))
+
+/** get the number of mixable outputs */
+#define MIXERIOCGETOUTPUTCOUNT	_MIXERIOC(0)
+
+/** reset (clear) the mixer configuration */
+#define MIXERIOCRESET		_MIXERIOC(1)
+
+/** simple channel scaler */
+struct mixer_scaler_s {
+	float			negative_scale;
+	float			positive_scale;
+	float			offset;
+	float			min_output;
+	float			max_output;
+};
+
+/** mixer input */
+struct mixer_control_s {
+	uint8_t			control_group;	/**< group from which the input reads */
+	uint8_t			control_index;	/**< index within the control group */
+	struct mixer_scaler_s 	scaler;		/**< scaling applied to the input before use */
+};
+
+/** simple mixer */
+struct mixer_simple_s {
+	uint8_t			control_count;	/**< number of inputs */
+	struct mixer_scaler_s	output_scaler;	/**< scaling for the output */
+	struct mixer_control_s	controls[0];	/**< actual size of the array is set by control_count */
+};
+
+#define MIXER_SIMPLE_SIZE(_icount)	(sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s))
+
+/**
+ * add a simple mixer in (struct mixer_simple_s *)arg
+ */
+#define MIXERIOCADDSIMPLE	_MIXERIOC(2)
+
+/* _MIXERIOC(3) was deprecated */
+/* _MIXERIOC(4) was deprecated */
+
+/**
+ * Add mixer(s) from the buffer in (const char *)arg
+ */
+#define MIXERIOCLOADBUF		_MIXERIOC(5)
+
+/*
+ * XXX Thoughts for additional operations:
+ *
+ * - get/set output scale, for tuning center/limit values.
+ * - save/serialise for saving tuned mixers.
+ */
+
+#endif /* _DRV_ACCEL_H */

src/drivers/drv_orb_dev.h

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.
+ *
+ ****************************************************************************/
+
+#ifndef _DRV_UORB_H
+#define _DRV_UORB_H
+
+/**
+ * @file uORB published object driver.
+ */
+
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <stdint.h>
+
+/* XXX for ORB_DECLARE used in many drivers */
+#include "../modules/uORB/uORB.h"
+
+/*
+ * ioctl() definitions
+ */
+
+/** path to the uORB control device for pub/sub topics */
+#define TOPIC_MASTER_DEVICE_PATH	"/obj/_obj_"
+
+/** path to the uORB control device for parameter topics */
+#define PARAM_MASTER_DEVICE_PATH	"/param/_param_"
+
+/** maximum ogbject name length */
+#define ORB_MAXNAME		32
+
+#define _ORBIOCBASE		(0x2600)
+#define _ORBIOC(_n)		(_IOC(_ORBIOCBASE, _n))
+
+/*
+ * IOCTLs for the uORB control device
+ */
+
+/** Advertise a new topic described by *(uorb_metadata *)arg */
+#define ORBIOCADVERTISE		_ORBIOC(0)
+
+/*
+ * IOCTLs for individual topics.
+ */
+
+/** Fetch the time at which the topic was last updated into *(uint64_t *)arg */
+#define ORBIOCLASTUPDATE	_ORBIOC(10)
+
+/** Check whether the topic has been updated since it was last read, sets *(bool *)arg */
+#define ORBIOCUPDATED		_ORBIOC(11)
+
+/** Set the minimum interval at which the topic can be seen to be updated for this subscription */
+#define ORBIOCSETINTERVAL	_ORBIOC(12)
+
+/** Get the global advertiser handle for the topic */
+#define ORBIOCGADVERTISER	_ORBIOC(13)
+
+#endif /* _DRV_UORB_H */

src/drivers/drv_pwm_output.h

@@ -0,0 +1,201 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 PWM servo output interface.
+ *
+ * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a
+ * pwm_output_values structure to the device, or by publishing to the
+ * output_pwm ORB topic.
+ * Writing a value of 0 to a channel suppresses any output for that
+ * channel.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+__BEGIN_DECLS
+
+/**
+ * Path for the default PWM output device.
+ *
+ * Note that on systems with more than one PWM output path (e.g.
+ * PX4FMU with PX4IO connected) there may be other devices that
+ * respond to this protocol.
+ */
+#define PWM_OUTPUT_DEVICE_PATH	"/dev/pwm_output"
+
+/**
+ * Maximum number of PWM output channels supported by the device.
+ */
+#define PWM_OUTPUT_MAX_CHANNELS	16
+
+/**
+ * Servo output signal type, value is actual servo output pulse
+ * width in microseconds.
+ */
+typedef uint16_t	servo_position_t;
+
+/**
+ * Servo output status structure.
+ *
+ * May be published to output_pwm, or written to a PWM output
+ * device.
+ */
+struct pwm_output_values {
+	/** desired pulse widths for each of the supported channels */
+	servo_position_t	values[PWM_OUTPUT_MAX_CHANNELS];
+};
+
+/*
+ * ORB tag for PWM outputs.
+ */
+ORB_DECLARE(output_pwm);
+
+/*
+ * ioctl() definitions
+ *
+ * Note that ioctls and ORB updates should not be mixed, as the
+ * behaviour of the system in this case is not defined.
+ */
+#define _PWM_SERVO_BASE		0x2a00
+
+/** arm all servo outputs handle by this driver */
+#define PWM_SERVO_ARM		_IOC(_PWM_SERVO_BASE, 0)
+
+/** disarm all servo outputs (stop generating pulses) */
+#define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
+
+/** set alternate servo update rate */
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+
+/** get the number of servos in *(unsigned *)arg */
+#define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 3)
+
+/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
+#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
+
+/** set a single servo to a specific value */
+#define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
+
+/** get a single specific servo value */
+#define PWM_SERVO_GET(_servo)	_IOC(_PWM_SERVO_BASE, 0x40 + _servo)
+
+/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels
+ *  whose update rates must be the same.
+ */
+#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n)
+
+/*
+ * Low-level PWM output interface.
+ *
+ * This is the low-level API to the platform-specific PWM driver.
+ */
+
+/**
+ * Intialise the PWM servo outputs using the specified configuration.
+ *
+ * @param channel_mask	Bitmask of channels (LSB = channel 0) to enable.
+ *			This allows some of the channels to remain configured
+ *			as GPIOs or as another function.
+ * @return		OK on success.
+ */
+__EXPORT extern int	up_pwm_servo_init(uint32_t channel_mask);
+
+/**
+ * De-initialise the PWM servo outputs.
+ */
+__EXPORT extern void	up_pwm_servo_deinit(void);
+
+/**
+ * Arm or disarm servo outputs.
+ *
+ * When disarmed, servos output no pulse.
+ *
+ * @bug This function should, but does not, guarantee that any pulse
+ *      currently in progress is cleanly completed.
+ *
+ * @param armed		If true, outputs are armed; if false they
+ *			are disarmed.
+ */
+__EXPORT extern void	up_pwm_servo_arm(bool armed);
+
+/**
+ * Set the servo update rate for all rate groups.
+ *
+ * @param rate		The update rate in Hz to set.
+ * @return		OK on success, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate(unsigned rate);
+
+/**
+ * Get a bitmap of output channels assigned to a given rate group.
+ *
+ * @param group		The rate group to query. Rate groups are assigned contiguously
+ *			starting from zero.
+ * @return		A bitmap of channels assigned to the rate group, or zero if
+ *			the group number has no channels.
+ */
+__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group);
+
+/**
+ * Set the update rate for a given rate group.
+ *
+ * @param group		The rate group whose update rate will be changed.
+ * @param rate		The update rate in Hz.
+ * @return		OK if the group was adjusted, -ERANGE if an unsupported update rate is set.
+ */
+__EXPORT extern int	up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate);
+
+/**
+ * Set the current output value for a channel.
+ *
+ * @param channel	The channel to set.
+ * @param value		The output pulse width in microseconds.
+ */
+__EXPORT extern int	up_pwm_servo_set(unsigned channel, servo_position_t value);
+
+/**
+ * Get the current output value for a channel.
+ *
+ * @param channel	The channel to read.
+ * @return		The output pulse width in microseconds, or zero if
+ *			outputs are not armed or not configured.
+ */
+__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
+
+__END_DECLS

src/drivers/drv_range_finder.h

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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
+ * 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 Rangefinder driver interface.
+ */
+
+#ifndef _DRV_RANGEFINDER_H
+#define _DRV_RANGEFINDER_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
+
+/**
+ * range finder report structure.  Reads from the device must be in multiples of this
+ * structure.
+ */
+struct range_finder_report {
+	uint64_t timestamp;
+	float distance; 			/** in meters */
+	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
+};
+
+/*
+ * ObjDev tag for raw range finder data.
+ */
+ORB_DECLARE(sensor_range_finder);
+
+/*
+ * ioctl() definitions
+ *
+ * Rangefinder drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _RANGEFINDERIOCBASE			(0x7900)
+#define __RANGEFINDERIOC(_n)		(_IOC(_RANGEFINDERIOCBASE, _n))
+
+/** set the minimum effective distance of the device */
+#define RANGEFINDERIOCSETMINIUMDISTANCE	__RANGEFINDERIOC(1)
+
+/** set the maximum effective distance of the device */
+#define RANGEFINDERIOCSETMAXIUMDISTANCE	__RANGEFINDERIOC(2)
+
+
+#endif /* _DRV_RANGEFINDER_H */

src/drivers/drv_rc_input.h

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 R/C input interface.
+ *
+ */
+
+#ifndef _DRV_RC_INPUT_H
+#define _DRV_RC_INPUT_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+/**
+ * Path for the default R/C input device.
+ *
+ * Note that on systems with more than one R/C input path (e.g.
+ * PX4FMU with PX4IO connected) there may be other devices that
+ * respond to this protocol.
+ *
+ * Input data may be obtained by subscribing to the input_rc
+ * object, or by poll/reading from the device.
+ */
+#define RC_INPUT_DEVICE_PATH	"/dev/input_rc"
+
+/**
+ * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
+ */
+#define RC_INPUT_MAX_CHANNELS	18
+
+/**
+ * Input signal type, value is a control position from zero to 100
+ * percent.
+ */
+typedef uint16_t		rc_input_t;
+
+enum RC_INPUT_SOURCE {
+	RC_INPUT_SOURCE_UNKNOWN = 0,
+	RC_INPUT_SOURCE_PX4FMU_PPM,
+	RC_INPUT_SOURCE_PX4IO_PPM,
+	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
+	RC_INPUT_SOURCE_PX4IO_SBUS
+};
+
+/**
+ * R/C input status structure.
+ *
+ * Published to input_rc, may also be published to other names depending
+ * on the board involved.
+ */
+struct rc_input_values {
+	/** decoding time */
+	uint64_t		timestamp;
+
+	/** number of channels actually being seen */
+	uint32_t		channel_count;
+
+	/** Input source */
+	enum RC_INPUT_SOURCE 	input_source;
+
+	/** measured pulse widths for each of the supported channels */
+	rc_input_t		values[RC_INPUT_MAX_CHANNELS];
+};
+
+/*
+ * ObjDev tag for R/C inputs.
+ */
+ORB_DECLARE(input_rc);
+
+#define _RC_INPUT_BASE		0x2b00
+
+/** Fetch R/C input values into (rc_input_values *)arg */
+
+#define RC_INPUT_GET			_IOC(_RC_INPUT_BASE, 0)
+
+
+#endif /* _DRV_RC_INPUT_H */

src/drivers/drv_sensor.h

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Common sensor API and ioctl definitions.
+ */
+
+#ifndef _DRV_SENSOR_H
+#define _DRV_SENSOR_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/*
+ * ioctl() definitions
+ *
+ * Note that a driver may not implement all of these operations, but
+ * if the operation is implemented it should conform to this API.
+ */
+
+#define _SENSORIOCBASE		(0x2000)
+#define _SENSORIOC(_n)		(_IOC(_SENSORIOCBASE, _n))
+
+/**
+ * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE
+ * constants
+ */
+#define SENSORIOCSPOLLRATE	_SENSORIOC(0)
+
+/**
+ * Return the driver's approximate polling rate in Hz, or one of the
+ * SENSOR_POLLRATE values.
+ */
+#define SENSORIOCGPOLLRATE	_SENSORIOC(1)
+
+#define SENSOR_POLLRATE_MANUAL		1000000	/**< poll when read */
+#define SENSOR_POLLRATE_EXTERNAL	1000001	/**< poll when device signals ready */
+#define SENSOR_POLLRATE_MAX		1000002	/**< poll at device maximum rate */
+#define SENSOR_POLLRATE_DEFAULT		1000003	/**< poll at driver normal rate */
+
+/**
+ * Set the internal queue depth to (arg) entries, must be at least 1
+ *
+ * This sets the upper bound on the number of readings that can be
+ * read from the driver.
+ */
+#define SENSORIOCSQUEUEDEPTH	_SENSORIOC(2)
+
+/** return the internal queue depth */
+#define SENSORIOCGQUEUEDEPTH	_SENSORIOC(3)
+
+/**
+ * Reset the sensor to its default configuration.
+ */
+#define SENSORIOCRESET		_SENSORIOC(4)
+
+#endif /* _DRV_SENSOR_H */

src/drivers/drv_tone_alarm.h

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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.
+ *
+ ****************************************************************************/
+
+/*
+ * Driver for the PX4 audio alarm port, /dev/tone_alarm.
+ *
+ * The tone_alarm driver supports a set of predefined "alarm"
+ * patterns and one user-supplied pattern.  Patterns are ordered by
+ * priority, with a higher-priority pattern interrupting any
+ * lower-priority pattern that might be playing.
+ *
+ * The TONE_SET_ALARM ioctl can be used to select a predefined
+ * alarm pattern, from 1 - <TBD>.  Selecting pattern zero silences
+ * the alarm.
+ *
+ * To supply a custom pattern, write an array of 1 - <TBD> tone_note
+ * structures to /dev/tone_alarm.  The custom pattern has a priority
+ * of zero.
+ *
+ * Patterns will normally play once and then silence (if a pattern
+ * was overridden it will not resume).  A pattern may be made to
+ * repeat by inserting a note with the duration set to
+ * DURATION_REPEAT.  This pattern will loop until either a
+ * higher-priority pattern is started or pattern zero is requested
+ * via the ioctl.
+ */
+
+#ifndef DRV_TONE_ALARM_H_
+#define DRV_TONE_ALARM_H_
+
+#include <sys/ioctl.h>
+
+#define _TONE_ALARM_BASE	0x7400
+#define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
+
+/* structure describing one note in a tone pattern */
+struct tone_note {
+	uint8_t		pitch;
+	uint8_t		duration;	/* duration in multiples of 10ms */
+#define DURATION_END		0	/* ends the pattern */
+#define DURATION_REPEAT		255	/* resets the note counter to zero */
+};
+
+enum tone_pitch {
+	TONE_NOTE_E4,   /* E4 */
+	TONE_NOTE_F4,   /* F4 */
+	TONE_NOTE_F4S,  /* F#4/Gb4 */
+	TONE_NOTE_G4,   /* G4 */
+	TONE_NOTE_G4S,  /* G#4/Ab4 */
+	TONE_NOTE_A4,   /* A4 */
+	TONE_NOTE_A4S,  /* A#4/Bb4 */
+	TONE_NOTE_B4,   /* B4 */
+	TONE_NOTE_C5,   /* C5 */
+	TONE_NOTE_C5S,  /* C#5/Db5 */
+	TONE_NOTE_D5,   /* D5 */
+	TONE_NOTE_D5S,  /* D#5/Eb5 */
+	TONE_NOTE_E5,   /* E5 */
+	TONE_NOTE_F5,   /* F5 */
+	TONE_NOTE_F5S,  /* F#5/Gb5 */
+	TONE_NOTE_G5,   /* G5 */
+	TONE_NOTE_G5S,  /* G#5/Ab5 */
+	TONE_NOTE_A5,   /* A5 */
+	TONE_NOTE_A5S,  /* A#5/Bb5 */
+	TONE_NOTE_B5,   /* B5 */
+	TONE_NOTE_C6,   /* C6 */
+	TONE_NOTE_C6S,  /* C#6/Db6 */
+	TONE_NOTE_D6,   /* D6 */
+	TONE_NOTE_D6S,  /* D#6/Eb6 */
+	TONE_NOTE_E6,   /* E6 */
+	TONE_NOTE_F6,   /* F6 */
+	TONE_NOTE_F6S,  /* F#6/Gb6 */
+	TONE_NOTE_G6,   /* G6 */
+	TONE_NOTE_G6S,  /* G#6/Ab6 */
+	TONE_NOTE_A6,   /* A6 */
+	TONE_NOTE_A6S,  /* A#6/Bb6 */
+	TONE_NOTE_B6,   /* B6 */
+	TONE_NOTE_C7,   /* C7 */
+	TONE_NOTE_C7S,  /* C#7/Db7 */
+	TONE_NOTE_D7,   /* D7 */
+	TONE_NOTE_D7S,  /* D#7/Eb7 */
+	TONE_NOTE_E7,   /* E7 */
+	TONE_NOTE_F7,   /* F7 */
+	TONE_NOTE_F7S,  /* F#7/Gb7 */
+	TONE_NOTE_G7,   /* G7 */
+	TONE_NOTE_G7S,  /* G#7/Ab7 */
+	TONE_NOTE_A7,   /* A7 */
+	TONE_NOTE_A7S,  /* A#7/Bb7 */
+	TONE_NOTE_B7,   /* B7 */
+	TONE_NOTE_C8,   /* C8 */
+	TONE_NOTE_C8S,  /* C#8/Db8 */
+	TONE_NOTE_D8,   /* D8 */
+	TONE_NOTE_D8S,  /* D#8/Eb8 */
+
+	TONE_NOTE_SILENCE,
+	TONE_NOTE_MAX
+};
+
+#endif /* DRV_TONE_ALARM_H_ */

src/drivers/led/Makefile

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# Makefile to build the LED driver.
+#
+
+include $(APPDIR)/mk/app.mk

src/drivers/led/led.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 led.cpp
+ *
+ * LED driver.
+ */
+
+#include <nuttx/config.h>
+#include <drivers/device/device.h>
+#include <drivers/drv_led.h>
+
+/* Ideally we'd be able to get these from up_internal.h */
+//#include <up_internal.h>
+__BEGIN_DECLS
+extern void up_ledinit();
+extern void up_ledon(int led);
+extern void up_ledoff(int led);
+__END_DECLS
+
+class LED : device::CDev
+{
+public:
+	LED();
+	virtual ~LED();
+
+	virtual int		init();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+};
+
+LED::LED() :
+	CDev("led", LED_DEVICE_PATH)
+{
+	// force immediate init/device registration
+	init();
+}
+
+LED::~LED()
+{
+}
+
+int
+LED::init()
+{
+	CDev::init();
+	up_ledinit();
+
+	return 0;
+}
+
+int
+LED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int result = OK;
+
+	switch (cmd) {
+	case LED_ON:
+		up_ledon(arg);
+		break;
+
+	case LED_OFF:
+		up_ledoff(arg);
+		break;
+
+	default:
+		result = CDev::ioctl(filp, cmd, arg);
+	}
+	return result;
+}
+
+namespace
+{
+LED	*gLED;
+}
+
+void
+drv_led_start()
+{
+	if (gLED == nullptr) {
+		gLED = new LED;
+		if (gLED != nullptr)
+			gLED->init();
+	}
+}

src/examples/math_demo/math_demo.cpp

@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: James Goppert
+ *
+ * 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 math_demo.cpp
+ * @author James Goppert
+ * Demonstration of math library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+
+/**
+ * Management function.
+ */
+extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
+
+/**
+ * Test function
+ */
+void test();
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+    if (reason)
+        fprintf(stderr, "%s\n", reason);
+    fprintf(stderr, "usage: math_demo {test}\n\n");
+    exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int math_demo_main(int argc, char *argv[])
+{
+
+    if (argc < 1)
+        usage("missing command");
+
+    if (!strcmp(argv[1], "test")) {
+        test();
+        exit(0);
+    }
+
+    usage("unrecognized command");
+    exit(1);
+}
+
+void test()
+{
+    printf("beginning math lib test\n");
+    using namespace math;
+    vectorTest();
+    matrixTest();
+    vector3Test();
+    eulerAnglesTest();
+    quaternionTest();
+    dcmTest();
+}

src/examples/math_demo/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# Mathlib / operations demo application
+#
+
+MODULE_COMMAND		= math_demo
+MODULE_STACKSIZE	= 12000
+
+SRCS			= math_demo.cpp

src/examples/px4_daemon_app/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# Daemon application
+#
+
+MODULE_COMMAND		= px4_daemon_app
+
+SRCS				= px4_daemon_app.c

src/examples/px4_daemon_app/px4_daemon_app.c

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Example User <mail@example.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_deamon_app.c
+ * Deamon application example for PX4 autopilot
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+__EXPORT int px4_deamon_app_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int px4_deamon_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int px4_deamon_app_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("deamon already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("deamon",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_DEFAULT,
+					 4096,
+					 px4_deamon_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tdeamon app is running\n");
+		} else {
+			printf("\tdeamon app not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int px4_deamon_thread_main(int argc, char *argv[]) {
+
+	printf("[deamon] starting\n");
+
+	thread_running = true;
+
+	while (!thread_should_exit) {
+		printf("Hello Deamon!\n");
+		sleep(10);
+	}
+
+	printf("[deamon] exiting.\n");
+
+	thread_running = false;
+
+	return 0;
+}

src/examples/px4_mavlink_debug/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+MODULE_COMMAND		= px4_mavlink_debug
+
+SRCS				= px4_mavlink_debug.c

src/examples/px4_mavlink_debug/px4_mavlink_debug.c

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Example User <mail@example.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_mavlink_debug.c
+ * Debug application example for PX4 autopilot
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/debug_key_value.h>
+
+__EXPORT int px4_mavlink_debug_main(int argc, char *argv[]);
+
+int px4_mavlink_debug_main(int argc, char *argv[])
+{
+	printf("Hello Debug!\n");
+
+	/* advertise debug value */
+	struct debug_key_value_s dbg = { .key = "velx", .value = 0.0f };
+	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
+
+	int value_counter = 0;
+
+	while (value_counter < 100) {
+		/* send one value */
+		dbg.value = value_counter;
+		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+
+		warnx("sent one more value..");
+
+		value_counter++;
+		usleep(500000);
+	}
+
+	return 0;
+}

src/examples/px4_simple_app/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+MODULE_COMMAND		= px4_simple_app
+
+SRCS				= px4_simple_app.c

src/examples/px4_simple_app/px4_simple_app.c

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Example User <mail@example.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_simple_app.c
+ * Minimal application example for PX4 autopilot
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+
+__EXPORT int px4_simple_app_main(int argc, char *argv[]);
+
+int px4_simple_app_main(int argc, char *argv[])
+{
+	printf("Hello Sky!\n");
+
+	/* subscribe to sensor_combined topic */
+	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
+	orb_set_interval(sensor_sub_fd, 1000);
+
+	/* advertise attitude topic */
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
+
+	/* one could wait for multiple topics with this technique, just using one here */
+	struct pollfd fds[] = {
+		{ .fd = sensor_sub_fd,   .events = POLLIN },
+		/* there could be more file descriptors here, in the form like:
+		 * { .fd = other_sub_fd,   .events = POLLIN },
+		 */
+	};
+
+	int error_counter = 0;
+
+	while (true) {
+		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
+		int poll_ret = poll(fds, 1, 1000);
+	 
+		/* handle the poll result */
+		if (poll_ret == 0) {
+			/* this means none of our providers is giving us data */
+			printf("[px4_simple_app] Got no data within a second\n");
+		} else if (poll_ret < 0) {
+			/* this is seriously bad - should be an emergency */
+			if (error_counter < 10 || error_counter % 50 == 0) {
+				/* use a counter to prevent flooding (and slowing us down) */
+				printf("[px4_simple_app] ERROR return value from poll(): %d\n"
+					, poll_ret);
+			}
+			error_counter++;
+		} else {
+	 
+			if (fds[0].revents & POLLIN) {
+				/* obtained data for the first file descriptor */
+				struct sensor_combined_s raw;
+				/* copy sensors raw data into local buffer */
+				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
+				printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
+					(double)raw.accelerometer_m_s2[0],
+					(double)raw.accelerometer_m_s2[1],
+					(double)raw.accelerometer_m_s2[2]);
+
+				/* set att and publish this information for other apps */
+				att.roll = raw.accelerometer_m_s2[0];
+				att.pitch = raw.accelerometer_m_s2[1];
+				att.yaw = raw.accelerometer_m_s2[2];
+				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
+			}
+			/* there could be more file descriptors here, in the form like:
+			 * if (fds[1..n].revents & POLLIN) {}
+			 */
+		}
+	}
+
+	return 0;
+}

src/modules/controllib/block/Block.cpp

@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Block.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "Block.hpp"
+#include "BlockParam.hpp"
+#include "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+Block::Block(SuperBlock *parent, const char *name) :
+	_name(name),
+	_parent(parent),
+	_dt(0),
+	_subscriptions(),
+	_params()
+{
+	if (getParent() != NULL) {
+		getParent()->getChildren().add(this);
+	}
+}
+
+void Block::getName(char *buf, size_t n)
+{
+	if (getParent() == NULL) {
+		strncpy(buf, _name, n);
+
+	} else {
+		char parentName[blockNameLengthMax];
+		getParent()->getName(parentName, n);
+
+		if (!strcmp(_name, "")) {
+			strncpy(buf, parentName, blockNameLengthMax);
+
+		} else {
+			snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
+		}
+	}
+}
+
+void Block::updateParams()
+{
+	BlockParamBase *param = getParams().getHead();
+	int count = 0;
+
+	while (param != NULL) {
+		if (count++ > maxParamsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max params for block: %s\n", name);
+			break;
+		}
+
+		//printf("updating param: %s\n", param->getName());
+		param->update();
+		param = param->getSibling();
+	}
+}
+
+void Block::updateSubscriptions()
+{
+	UOrbSubscriptionBase *sub = getSubscriptions().getHead();
+	int count = 0;
+
+	while (sub != NULL) {
+		if (count++ > maxSubscriptionsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max subscriptions for block: %s\n", name);
+			break;
+		}
+
+		sub->update();
+		sub = sub->getSibling();
+	}
+}
+
+void Block::updatePublications()
+{
+	UOrbPublicationBase *pub = getPublications().getHead();
+	int count = 0;
+
+	while (pub != NULL) {
+		if (count++ > maxPublicationsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max publications for block: %s\n", name);
+			break;
+		}
+
+		pub->update();
+		pub = pub->getSibling();
+	}
+}
+
+void SuperBlock::setDt(float dt)
+{
+	Block::setDt(dt);
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->setDt(dt);
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildParams()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updateParams();
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildSubscriptions()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updateSubscriptions();
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildPublications()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updatePublications();
+		child = child->getSibling();
+	}
+}
+
+} // namespace control

src/modules/controllib/block/Block.hpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Block.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <inttypes.h>
+
+#include "List.hpp"
+
+namespace control
+{
+
+static const uint16_t maxChildrenPerBlock = 100;
+static const uint16_t maxParamsPerBlock = 100;
+static const uint16_t maxSubscriptionsPerBlock = 100;
+static const uint16_t maxPublicationsPerBlock = 100;
+static const uint8_t blockNameLengthMax = 80;
+
+// forward declaration
+class BlockParamBase;
+class UOrbSubscriptionBase;
+class UOrbPublicationBase;
+class SuperBlock;
+
+/**
+ */
+class __EXPORT Block :
+	public ListNode<Block *>
+{
+public:
+	friend class BlockParamBase;
+// methods
+	Block(SuperBlock *parent, const char *name);
+	void getName(char *name, size_t n);
+	virtual ~Block() {};
+	virtual void updateParams();
+	virtual void updateSubscriptions();
+	virtual void updatePublications();
+	virtual void setDt(float dt) { _dt = dt; }
+// accessors
+	float getDt() { return _dt; }
+protected:
+// accessors
+	SuperBlock *getParent() { return _parent; }
+	List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
+	List<UOrbPublicationBase *> & getPublications() { return _publications; }
+	List<BlockParamBase *> & getParams() { return _params; }
+// attributes
+	const char *_name;
+	SuperBlock *_parent;
+	float _dt;
+	List<UOrbSubscriptionBase *> _subscriptions;
+	List<UOrbPublicationBase *> _publications;
+	List<BlockParamBase *> _params;
+};
+
+class __EXPORT SuperBlock :
+	public Block
+{
+public:
+	friend class Block;
+// methods
+	SuperBlock(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_children() {
+	}
+	virtual ~SuperBlock() {};
+	virtual void setDt(float dt);
+	virtual void updateParams() {
+		Block::updateParams();
+
+		if (getChildren().getHead() != NULL) updateChildParams();
+	}
+	virtual void updateSubscriptions() {
+		Block::updateSubscriptions();
+
+		if (getChildren().getHead() != NULL) updateChildSubscriptions();
+	}
+	virtual void updatePublications() {
+		Block::updatePublications();
+
+		if (getChildren().getHead() != NULL) updateChildPublications();
+	}
+protected:
+// methods
+	List<Block *> & getChildren() { return _children; }
+	void updateChildParams();
+	void updateChildSubscriptions();
+	void updateChildPublications();
+// attributes
+	List<Block *> _children;
+};
+
+} // namespace control

src/modules/controllib/block/BlockParam.cpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Blockparam.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "BlockParam.hpp"
+
+namespace control
+{
+
+BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) :
+	_handle(PARAM_INVALID)
+{
+	char fullname[blockNameLengthMax];
+
+	if (parent == NULL) {
+		strncpy(fullname, name, blockNameLengthMax);
+
+	} else {
+		char parentName[blockNameLengthMax];
+		parent->getName(parentName, blockNameLengthMax);
+
+		if (!strcmp(name, "")) {
+			strncpy(fullname, parentName, blockNameLengthMax);
+
+		} else if (parent_prefix) {
+			snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
+		} else {
+			strncpy(fullname, name, blockNameLengthMax);
+		}
+
+		parent->getParams().add(this);
+	}
+
+	_handle = param_find(fullname);
+
+	if (_handle == PARAM_INVALID)
+		printf("error finding param: %s\n", fullname);
+};
+
+} // namespace control

src/modules/controllib/block/BlockParam.hpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 BlockParam.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <systemlib/param/param.h>
+
+#include "Block.hpp"
+#include "List.hpp"
+
+namespace control
+{
+
+/**
+ * A base class for block params that enables traversing linked list.
+ */
+class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
+{
+public:
+	/**
+	 * Instantiate a block param base.
+	 *
+	 * @param parent_prefix Set to true to include the parent name in the parameter name
+	 */
+	BlockParamBase(Block *parent, const char *name, bool parent_prefix=true);
+	virtual ~BlockParamBase() {};
+	virtual void update() = 0;
+	const char *getName() { return param_name(_handle); }
+protected:
+	param_t _handle;
+};
+
+/**
+ * Parameters that are tied to blocks for updating and nameing.
+ */
+template<class T>
+class __EXPORT BlockParam : public BlockParamBase
+{
+public:
+	BlockParam(Block *block, const char *name, bool parent_prefix=true) :
+		BlockParamBase(block, name, parent_prefix),
+		_val() {
+		update();
+	}
+	T get() { return _val; }
+	void set(T val) { _val = val; }
+	void update() {
+		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+	}
+protected:
+	T _val;
+};
+
+} // namespace control

src/modules/controllib/block/List.hpp

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 Node.h
+ *
+ * A node of a linked list.
+ */
+
+#pragma once
+
+template<class T>
+class __EXPORT ListNode
+{
+public:
+	ListNode() : _sibling(NULL) {
+	}
+	void setSibling(T sibling) { _sibling = sibling; }
+	T getSibling() { return _sibling; }
+	T get() {
+		return _sibling;
+	}
+protected:
+	T _sibling;
+};
+
+template<class T>
+class __EXPORT List
+{
+public:
+	List() : _head() {
+	}
+	void add(T newNode) {
+		newNode->setSibling(getHead());
+		setHead(newNode);
+	}
+	T getHead() { return _head; }
+private:
+	void setHead(T &head) { _head = head; }
+	T _head;
+};

src/modules/controllib/block/UOrbPublication.cpp

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 UOrbPublication.cpp
+ *
+ */
+
+#include "UOrbPublication.hpp"

src/modules/controllib/block/UOrbPublication.hpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 UOrbPublication.h
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include "Block.hpp"
+#include "List.hpp"
+
+
+namespace control
+{
+
+class Block;
+
+/**
+ * Base publication warapper class, used in list traversal
+ * of various publications.
+ */
+class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
+{
+public:
+
+	UOrbPublicationBase(
+		List<UOrbPublicationBase *> * list,
+		const struct orb_metadata *meta) :
+		_meta(meta),
+		_handle() {
+		if (list != NULL) list->add(this);
+	}
+	void update() {
+		orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+	}
+	virtual void *getDataVoidPtr() = 0;
+	virtual ~UOrbPublicationBase() {
+		orb_unsubscribe(getHandle());
+	}
+	const struct orb_metadata *getMeta() { return _meta; }
+	int getHandle() { return _handle; }
+protected:
+	void setHandle(orb_advert_t handle) { _handle = handle; }
+	const struct orb_metadata *_meta;
+	orb_advert_t _handle;
+};
+
+/**
+ * UOrb Publication wrapper class
+ */
+template<class T>
+class UOrbPublication :
+	public T, // this must be first!
+	public UOrbPublicationBase
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param list      A list interface for adding to list during construction
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			for the topic.
+	 */
+	UOrbPublication(
+		List<UOrbPublicationBase *> * list,
+		const struct orb_metadata *meta) :
+		T(), // initialize data structure to zero
+		UOrbPublicationBase(list, meta) {
+		// It is important that we call T()
+		// before we publish the data, so we
+		// call this here instead of the base class
+		setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+	}
+	virtual ~UOrbPublication() {}
+	/*
+	 * XXX
+	 * This function gets the T struct, assuming
+	 * the struct is the first base class, this
+	 * should use dynamic cast, but doesn't
+	 * seem to be available
+	 */
+	void *getDataVoidPtr() { return (void *)(T *)(this); }
+};
+
+} // namespace control

src/modules/controllib/block/UOrbSubscription.cpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 UOrbSubscription.cpp
+ *
+ */
+
+#include "UOrbSubscription.hpp"
+
+namespace control
+{
+
+bool __EXPORT UOrbSubscriptionBase::updated()
+{
+	bool isUpdated = false;
+	orb_check(_handle, &isUpdated);
+	return isUpdated;
+}
+
+} // namespace control

src/modules/controllib/block/UOrbSubscription.hpp

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 UOrbSubscription.h
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include "Block.hpp"
+#include "List.hpp"
+
+
+namespace control
+{
+
+class Block;
+
+/**
+ * Base subscription warapper class, used in list traversal
+ * of various subscriptions.
+ */
+class __EXPORT UOrbSubscriptionBase :
+	public ListNode<control::UOrbSubscriptionBase *>
+{
+public:
+// methods
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			for the topic.
+	 */
+	UOrbSubscriptionBase(
+		List<UOrbSubscriptionBase *> * list,
+		const struct orb_metadata *meta) :
+		_meta(meta),
+		_handle() {
+		if (list != NULL) list->add(this);
+	}
+	bool updated();
+	void update() {
+		if (updated()) {
+			orb_copy(_meta, _handle, getDataVoidPtr());
+		}
+	}
+	virtual void *getDataVoidPtr() = 0;
+	virtual ~UOrbSubscriptionBase() {
+		orb_unsubscribe(_handle);
+	}
+// accessors
+	const struct orb_metadata *getMeta() { return _meta; }
+	int getHandle() { return _handle; }
+protected:
+// accessors
+	void setHandle(int handle) { _handle = handle; }
+// attributes
+	const struct orb_metadata *_meta;
+	int _handle;
+};
+
+/**
+ * UOrb Subscription wrapper class
+ */
+template<class T>
+class __EXPORT UOrbSubscription :
+	public T, // this must be first!
+	public UOrbSubscriptionBase
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param list      A list interface for adding to list during construction
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			for the topic.
+	 * @param interval  The minimum interval in milliseconds between updates
+	 */
+	UOrbSubscription(
+		List<UOrbSubscriptionBase *> * list,
+		const struct orb_metadata *meta, unsigned interval) :
+		T(), // initialize data structure to zero
+		UOrbSubscriptionBase(list, meta) {
+		setHandle(orb_subscribe(getMeta()));
+		orb_set_interval(getHandle(), interval);
+	}
+
+	/**
+	 * Deconstructor
+	 */
+	virtual ~UOrbSubscription() {}
+
+	/*
+	 * XXX
+	 * This function gets the T struct, assuming
+	 * the struct is the first base class, this
+	 * should use dynamic cast, but doesn't
+	 * seem to be available
+	 */
+	void *getDataVoidPtr() { return (void *)(T *)(this); }
+	T getData() { return T(*this); }
+};
+
+} // namespace control

src/modules/controllib/blocks.cpp

@@ -0,0 +1,486 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 blocks.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <stdio.h>
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+int basicBlocksTest()
+{
+	blockLimitTest();
+	blockLimitSymTest();
+	blockLowPassTest();
+	blockHighPassTest();
+	blockIntegralTest();
+	blockIntegralTrapTest();
+	blockDerivativeTest();
+	blockPTest();
+	blockPITest();
+	blockPDTest();
+	blockPIDTest();
+	blockOutputTest();
+	blockRandUniformTest();
+	blockRandGaussTest();
+	return 0;
+}
+
+float BlockLimit::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < getMin()) {
+		input = getMin();
+	}
+
+	return input;
+}
+
+int blockLimitTest()
+{
+	printf("Test BlockLimit\t\t\t: ");
+	BlockLimit limit(NULL, "TEST");
+	// initial state
+	ASSERT(equal(1.0f, limit.getMax()));
+	ASSERT(equal(-1.0f, limit.getMin()));
+	ASSERT(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT(equal(1.0f, limit.update(2.0f)));
+	ASSERT(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockLimitSym::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < -getMax()) {
+		input = -getMax();
+	}
+
+	return input;
+}
+
+int blockLimitSymTest()
+{
+	printf("Test BlockLimitSym\t\t: ");
+	BlockLimitSym limit(NULL, "TEST");
+	// initial state
+	ASSERT(equal(1.0f, limit.getMax()));
+	ASSERT(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT(equal(1.0f, limit.update(2.0f)));
+	ASSERT(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockLowPass::update(float input)
+{
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = b / (1 + b);
+	setState(a * input + (1 - a)*getState());
+	return getState();
+}
+
+int blockLowPassTest()
+{
+	printf("Test BlockLowPass\t\t: ");
+	BlockLowPass lowPass(NULL, "TEST_LP");
+	// test initial state
+	ASSERT(equal(10.0f, lowPass.getFCut()));
+	ASSERT(equal(0.0f, lowPass.getState()));
+	ASSERT(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT(equal(2.0f, lowPass.getState()));
+	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
+float BlockHighPass::update(float input)
+{
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = 1 / (1 + b);
+	setY(a * (getY() + input - getU()));
+	setU(input);
+	return getY();
+}
+
+int blockHighPassTest()
+{
+	printf("Test BlockHighPass\t\t: ");
+	BlockHighPass highPass(NULL, "TEST_HP");
+	// test initial state
+	ASSERT(equal(10.0f, highPass.getFCut()));
+	ASSERT(equal(0.0f, highPass.getU()));
+	ASSERT(equal(0.0f, highPass.getY()));
+	ASSERT(equal(0.0f, highPass.getDt()));
+	// set dt
+	highPass.setDt(0.1f);
+	ASSERT(equal(0.1f, highPass.getDt()));
+	// set state
+	highPass.setU(1.0f);
+	ASSERT(equal(1.0f, highPass.getU()));
+	highPass.setY(1.0f);
+	ASSERT(equal(1.0f, highPass.getY()));
+	// test update
+	ASSERT(equal(0.2746051f, highPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		highPass.update(2.0f);
+	}
+
+	ASSERT(equal(0.0f, highPass.getY()));
+	ASSERT(equal(0.0f, highPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockIntegral::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() + input * getDt()));
+	return getY();
+}
+
+int blockIntegralTest()
+{
+	printf("Test BlockIntegral\t\t: ");
+	BlockIntegral integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT(equal(1.0f, integral.getMax()));
+	ASSERT(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT(equal(0.1f, integral.getDt()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setY(-0.9f);
+	ASSERT(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setY(0.1f);
+	ASSERT(equal(0.2f, integral.update(1.0)));
+	ASSERT(equal(0.2f, integral.getY()));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockIntegralTrap::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() +
+			   (getU() + input) / 2.0f * getDt()));
+	setU(input);
+	return getY();
+}
+
+int blockIntegralTrapTest()
+{
+	printf("Test BlockIntegralTrap\t\t: ");
+	BlockIntegralTrap integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT(equal(1.0f, integral.getMax()));
+	ASSERT(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT(equal(0.1f, integral.getDt()));
+	// set U
+	integral.setU(1.0f);
+	ASSERT(equal(1.0f, integral.getU()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setU(-1.0f);
+	integral.setY(-0.9f);
+	ASSERT(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setU(2.0f);
+	integral.setY(0.1f);
+	ASSERT(equal(0.25f, integral.update(1.0)));
+	ASSERT(equal(0.25f, integral.getY()));
+	ASSERT(equal(1.0f, integral.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockDerivative::update(float input)
+{
+	float output = _lowPass.update((input - getU()) / getDt());
+	setU(input);
+	return output;
+}
+
+int blockDerivativeTest()
+{
+	printf("Test BlockDerivative\t\t: ");
+	BlockDerivative derivative(NULL, "TEST_D");
+	// test initial state
+	ASSERT(equal(0.0f, derivative.getU()));
+	ASSERT(equal(10.0f, derivative.getLP()));
+	// set dt
+	derivative.setDt(0.1f);
+	ASSERT(equal(0.1f, derivative.getDt()));
+	// set U
+	derivative.setU(1.0f);
+	ASSERT(equal(1.0f, derivative.getU()));
+	// test  update
+	ASSERT(equal(8.6269744f, derivative.update(2.0f)));
+	ASSERT(equal(2.0f, derivative.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPTest()
+{
+	printf("Test BlockP\t\t\t: ");
+	BlockP blockP(NULL, "TEST_P");
+	// test initial state
+	ASSERT(equal(0.2f, blockP.getKP()));
+	ASSERT(equal(0.0f, blockP.getDt()));
+	// set dt
+	blockP.setDt(0.1f);
+	ASSERT(equal(0.1f, blockP.getDt()));
+	// test  update
+	ASSERT(equal(0.4f, blockP.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPITest()
+{
+	printf("Test BlockPI\t\t\t: ");
+	BlockPI blockPI(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPI.getKP()));
+	ASSERT(equal(0.1f, blockPI.getKI()));
+	ASSERT(equal(0.0f, blockPI.getDt()));
+	ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
+	// set dt
+	blockPI.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPI.getDt()));
+	// set integral state
+	blockPI.getIntegral().setY(0.1f);
+	ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
+	ASSERT(equal(0.43f, blockPI.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPDTest()
+{
+	printf("Test BlockPD\t\t\t: ");
+	BlockPD blockPD(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPD.getKP()));
+	ASSERT(equal(0.01f, blockPD.getKD()));
+	ASSERT(equal(0.0f, blockPD.getDt()));
+	ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
+	// set dt
+	blockPD.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPD.getDt()));
+	// set derivative state
+	blockPD.getDerivative().setU(1.0f);
+	ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
+	// test  update
+	// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
+	ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPIDTest()
+{
+	printf("Test BlockPID\t\t\t: ");
+	BlockPID blockPID(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPID.getKP()));
+	ASSERT(equal(0.1f, blockPID.getKI()));
+	ASSERT(equal(0.01f, blockPID.getKD()));
+	ASSERT(equal(0.0f, blockPID.getDt()));
+	ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
+	ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
+	// set dt
+	blockPID.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPID.getDt()));
+	// set derivative state
+	blockPID.getDerivative().setU(1.0f);
+	ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
+	// set integral state
+	blockPID.getIntegral().setY(0.1f);
+	ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
+	ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockOutputTest()
+{
+	printf("Test BlockOutput\t\t: ");
+	BlockOutput blockOutput(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockOutput.getDt()));
+	ASSERT(equal(0.5f, blockOutput.get()));
+	ASSERT(equal(-1.0f, blockOutput.getMin()));
+	ASSERT(equal(1.0f, blockOutput.getMax()));
+	// test update below min
+	blockOutput.update(-2.0f);
+	ASSERT(equal(-1.0f, blockOutput.get()));
+	// test update above max
+	blockOutput.update(2.0f);
+	ASSERT(equal(1.0f, blockOutput.get()));
+	// test trim
+	blockOutput.update(0.0f);
+	ASSERT(equal(0.5f, blockOutput.get()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandUniformTest()
+{
+	srand(1234);
+	printf("Test BlockRandUniform\t\t: ");
+	BlockRandUniform blockRandUniform(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockRandUniform.getDt()));
+	ASSERT(equal(-1.0f, blockRandUniform.getMin()));
+	ASSERT(equal(1.0f, blockRandUniform.getMax()));
+	// test update
+	int n = 10000;
+	float mean = blockRandUniform.update();
+
+	// recursive mean algorithm from Knuth
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandUniform.update();
+		mean += (val - mean) / i;
+		ASSERT(val <= blockRandUniform.getMax());
+		ASSERT(val >= blockRandUniform.getMin());
+	}
+
+	ASSERT(equal(mean, (blockRandUniform.getMin() +
+			    blockRandUniform.getMax()) / 2, 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandGaussTest()
+{
+	srand(1234);
+	printf("Test BlockRandGauss\t\t: ");
+	BlockRandGauss blockRandGauss(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockRandGauss.getDt()));
+	ASSERT(equal(1.0f, blockRandGauss.getMean()));
+	ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
+	// test update
+	int n = 10000;
+	float mean = blockRandGauss.update();
+	float sum = 0;
+
+	// recursive mean, stdev algorithm from Knuth
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandGauss.update();
+		float newMean = mean + (val - mean) / i;
+		sum += (val - mean) * (val - newMean);
+		mean = newMean;
+	}
+
+	float stdDev = sqrt(sum / (n - 1));
+	ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
+	ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace control

src/modules/controllib/blocks.hpp

@@ -0,0 +1,494 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 blocks.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <assert.h>
+#include <time.h>
+#include <stdlib.h>
+#include <mathlib/math/test/test.hpp>
+
+#include "block/Block.hpp"
+#include "block/BlockParam.hpp"
+
+namespace control
+{
+
+int __EXPORT basicBlocksTest();
+
+/**
+ * A limiter/ saturation.
+ * The output of update is the input, bounded
+ * by min/max.
+ */
+class __EXPORT BlockLimit : public Block
+{
+public:
+// methods
+	BlockLimit(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_min(this, "MIN"),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockLimit() {};
+	float update(float input);
+// accessors
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+protected:
+// attributes
+	BlockParam<float> _min;
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockLimitTest();
+
+/**
+ * A symmetric limiter/ saturation.
+ * Same as limiter but with only a max, is used for
+ * upper limit of +max, and lower limit of -max
+ */
+class __EXPORT BlockLimitSym : public Block
+{
+public:
+// methods
+	BlockLimitSym(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockLimitSym() {};
+	float update(float input);
+// accessors
+	float getMax() { return _max.get(); }
+protected:
+// attributes
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockLimitSymTest();
+
+/**
+ * A low pass filter as described here:
+ * http://en.wikipedia.org/wiki/Low-pass_filter.
+ */
+class __EXPORT BlockLowPass : public Block
+{
+public:
+// methods
+	BlockLowPass(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_state(0),
+		_fCut(this, "") // only one parameter, no need to name
+	{};
+	virtual ~BlockLowPass() {};
+	float update(float input);
+// accessors
+	float getState() { return _state; }
+	float getFCut() { return _fCut.get(); }
+	void setState(float state) { _state = state; }
+protected:
+// attributes
+	float _state;
+	BlockParam<float> _fCut;
+};
+
+int __EXPORT blockLowPassTest();
+
+/**
+ * A high pass filter as described here:
+ * http://en.wikipedia.org/wiki/High-pass_filter.
+ */
+class __EXPORT BlockHighPass : public Block
+{
+public:
+// methods
+	BlockHighPass(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_u(0),
+		_y(0),
+		_fCut(this, "") // only one parameter, no need to name
+	{};
+	virtual ~BlockHighPass() {};
+	float update(float input);
+// accessors
+	float getU() {return _u;}
+	float getY() {return _y;}
+	float getFCut() {return _fCut.get();}
+	void setU(float u) {_u = u;}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _u; /**< previous input */
+	float _y; /**< previous output */
+	BlockParam<float> _fCut; /**< cut-off frequency, Hz */
+};
+
+int __EXPORT blockHighPassTest();
+
+/**
+ * A rectangular integrator.
+ * A limiter is built into the class to bound the
+ * integral's internal state. This is important
+ * for windup protection.
+ * @see Limit
+ */
+class __EXPORT BlockIntegral: public SuperBlock
+{
+public:
+// methods
+	BlockIntegral(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_y(0),
+		_limit(this, "") {};
+	virtual ~BlockIntegral() {};
+	float update(float input);
+// accessors
+	float getY() {return _y;}
+	float getMax() {return _limit.getMax();}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _y; /**< previous output */
+	BlockLimitSym _limit; /**< limiter */
+};
+
+int __EXPORT blockIntegralTest();
+
+/**
+ * A trapezoidal integrator.
+ * http://en.wikipedia.org/wiki/Trapezoidal_rule
+ * A limiter is built into the class to bound the
+ * integral's internal state. This is important
+ * for windup protection.
+ * @see Limit
+ */
+class __EXPORT BlockIntegralTrap : public SuperBlock
+{
+public:
+// methods
+	BlockIntegralTrap(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_u(0),
+		_y(0),
+		_limit(this, "") {};
+	virtual ~BlockIntegralTrap() {};
+	float update(float input);
+// accessors
+	float getU() {return _u;}
+	float getY() {return _y;}
+	float getMax() {return _limit.getMax();}
+	void setU(float u) {_u = u;}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _u; /**< previous input */
+	float _y; /**< previous output */
+	BlockLimitSym _limit; /**< limiter */
+};
+
+int __EXPORT blockIntegralTrapTest();
+
+/**
+ * A simple derivative approximation.
+ * This uses the previous and current input.
+ * This has a built in low pass filter.
+ * @see LowPass
+ */
+class __EXPORT BlockDerivative : public SuperBlock
+{
+public:
+// methods
+	BlockDerivative(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_u(0),
+		_lowPass(this, "LP")
+	{};
+	virtual ~BlockDerivative() {};
+	float update(float input);
+// accessors
+	void setU(float u) {_u = u;}
+	float getU() {return _u;}
+	float getLP() {return _lowPass.getFCut();}
+protected:
+// attributes
+	float _u; /**< previous input */
+	BlockLowPass _lowPass; /**< low pass filter */
+};
+
+int __EXPORT blockDerivativeTest();
+
+/**
+ * A proportional controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockP: public Block
+{
+public:
+// methods
+	BlockP(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_kP(this, "") // only one param, no need to name
+	{};
+	virtual ~BlockP() {};
+	float update(float input) {
+		return getKP() * input;
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+protected:
+	BlockParam<float> _kP;
+};
+
+int __EXPORT blockPTest();
+
+/**
+ * A proportional-integral controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPI: public SuperBlock
+{
+public:
+// methods
+	BlockPI(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_integral(this, "I"),
+		_kP(this, "P"),
+		_kI(this, "I")
+	{};
+	virtual ~BlockPI() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKI() * getIntegral().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	BlockIntegral &getIntegral() { return _integral; }
+private:
+	BlockIntegral _integral;
+	BlockParam<float> _kP;
+	BlockParam<float> _kI;
+};
+
+int __EXPORT blockPITest();
+
+/**
+ * A proportional-derivative controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPD: public SuperBlock
+{
+public:
+// methods
+	BlockPD(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_derivative(this, "D"),
+		_kP(this, "P"),
+		_kD(this, "D")
+	{};
+	virtual ~BlockPD() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKD() * getDerivative().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKD() { return _kD.get(); }
+	BlockDerivative &getDerivative() { return _derivative; }
+private:
+	BlockDerivative _derivative;
+	BlockParam<float> _kP;
+	BlockParam<float> _kD;
+};
+
+int __EXPORT blockPDTest();
+
+/**
+ * A proportional-integral-derivative controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPID: public SuperBlock
+{
+public:
+// methods
+	BlockPID(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_integral(this, "I"),
+		_derivative(this, "D"),
+		_kP(this, "P"),
+		_kI(this, "I"),
+		_kD(this, "D")
+	{};
+	virtual ~BlockPID() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKI() * getIntegral().update(input) +
+		       getKD() * getDerivative().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	float getKD() { return _kD.get(); }
+	BlockIntegral &getIntegral() { return _integral; }
+	BlockDerivative &getDerivative() { return _derivative; }
+private:
+// attributes
+	BlockIntegral _integral;
+	BlockDerivative _derivative;
+	BlockParam<float> _kP;
+	BlockParam<float> _kI;
+	BlockParam<float> _kD;
+};
+
+int __EXPORT blockPIDTest();
+
+/**
+ * An output trim/ saturation block
+ */
+class __EXPORT BlockOutput: public SuperBlock
+{
+public:
+// methods
+	BlockOutput(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_trim(this, "TRIM"),
+		_limit(this, ""),
+		_val(0) {
+		update(0);
+	};
+	virtual ~BlockOutput() {};
+	void update(float input) {
+		_val = _limit.update(input + getTrim());
+	}
+// accessors
+	float getMin() { return _limit.getMin(); }
+	float getMax() { return _limit.getMax(); }
+	float getTrim() { return _trim.get(); }
+	float get() { return _val; }
+private:
+// attributes
+	BlockParam<float> _trim;
+	BlockLimit _limit;
+	float _val;
+};
+
+int __EXPORT blockOutputTest();
+
+/**
+ * A uniform random number generator
+ */
+class __EXPORT BlockRandUniform: public Block
+{
+public:
+// methods
+	BlockRandUniform(SuperBlock *parent,
+			 const char *name) :
+		Block(parent, name),
+		_min(this, "MIN"),
+		_max(this, "MAX") {
+		// seed should be initialized somewhere
+		// in main program for all calls to rand
+		// XXX currently in nuttx if you seed to 0, rand breaks
+	};
+	virtual ~BlockRandUniform() {};
+	float update() {
+		static float rand_max = MAX_RAND;
+		float rand_val = rand();
+		float bounds = getMax() - getMin();
+		return getMin() + (rand_val * bounds) / rand_max;
+	}
+// accessors
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+private:
+// attributes
+	BlockParam<float> _min;
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockRandUniformTest();
+
+class __EXPORT BlockRandGauss: public Block
+{
+public:
+// methods
+	BlockRandGauss(SuperBlock *parent,
+		       const char *name) :
+		Block(parent, name),
+		_mean(this, "MEAN"),
+		_stdDev(this, "DEV") {
+		// seed should be initialized somewhere
+		// in main program for all calls to rand
+		// XXX currently in nuttx if you seed to 0, rand breaks
+	};
+	virtual ~BlockRandGauss() {};
+	float update() {
+		static float V1, V2, S;
+		static int phase = 0;
+		float X;
+
+		if (phase == 0) {
+			do {
+				float U1 = (float)rand() / MAX_RAND;
+				float U2 = (float)rand() / MAX_RAND;
+				V1 = 2 * U1 - 1;
+				V2 = 2 * U2 - 1;
+				S = V1 * V1 + V2 * V2;
+			} while (S >= 1 || fabsf(S) < 1e-8f);
+
+			X = V1 * float(sqrt(-2 * float(log(S)) / S));
+
+		} else
+			X = V2 * float(sqrt(-2 * float(log(S)) / S));
+
+		phase = 1 - phase;
+		return X * getStdDev() + getMean();
+	}
+// accessors
+	float getMean() { return _mean.get(); }
+	float getStdDev() { return _stdDev.get(); }
+private:
+// attributes
+	BlockParam<float> _mean;
+	BlockParam<float> _stdDev;
+};
+
+int __EXPORT blockRandGaussTest();
+
+} // namespace control

src/modules/controllib/fixedwing.cpp

@@ -0,0 +1,361 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 fixedwing.cpp
+ *
+ * Controller library code
+ */
+
+#include "fixedwing.hpp"
+
+namespace control
+{
+
+namespace fixedwing
+{
+
+BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_rLowPass(this, "R_LP"),
+	_rWashout(this, "R_HP"),
+	_r2Rdr(this, "R2RDR"),
+	_rudder(0)
+{
+}
+
+BlockYawDamper::~BlockYawDamper() {};
+
+void BlockYawDamper::update(float rCmd, float r)
+{
+	_rudder = _r2Rdr.update(rCmd -
+				_rWashout.update(_rLowPass.update(r)));
+}
+
+BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_yawDamper(this, ""),
+	_pLowPass(this, "P_LP"),
+	_qLowPass(this, "Q_LP"),
+	_p2Ail(this, "P2AIL"),
+	_q2Elv(this, "Q2ELV"),
+	_aileron(0),
+	_elevator(0)
+{
+}
+
+BlockStabilization::~BlockStabilization() {};
+
+void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
+				float p, float q, float r)
+{
+	_aileron = _p2Ail.update(
+			   pCmd - _pLowPass.update(p));
+	_elevator = _q2Elv.update(
+			    qCmd - _qLowPass.update(q));
+	_yawDamper.update(rCmd, r);
+}
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
+	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
+	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
+	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
+	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
+	BlockUorbEnabledAutopilot(parent, name),
+	_stabilization(this, ""), // no name needed, already unique
+
+	// heading hold
+	_psi2Phi(this, "PSI2PHI"),
+	_phi2P(this, "PHI2P"),
+	_phiLimit(this, "PHI_LIM"),
+
+	// velocity hold
+	_v2Theta(this, "V2THE"),
+	_theta2Q(this, "THE2Q"),
+	_theLimit(this, "THE"),
+	_vLimit(this, "V"),
+
+	// altitude/roc hold
+	_h2Thr(this, "H2THR"),
+	_roc2Thr(this, "ROC2THR"),
+
+	// guidance block
+	_guide(this, ""),
+
+	// block params
+	_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
+	_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
+	_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
+	_trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+
+	_vCmd(this, "V_CMD"),
+	_rocMax(this, "ROC_MAX"),
+	_attPoll(),
+	_lastPosCmd(),
+	_timeStamp(0)
+{
+	_attPoll.fd = _att.getHandle();
+	_attPoll.events = POLLIN;
+}
+
+void BlockMultiModeBacksideAutopilot::update()
+{
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// store old position command before update if new command sent
+	if (_posCmd.updated()) {
+		_lastPosCmd = _posCmd.getData();
+	}
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// only update guidance in auto mode
+	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+	}
+
+	// XXX handle STABILIZED (loiter on spot) as well
+	// once the system switches from manual or auto to stabilized
+	// the setpoint should update to loitering around this position
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_AUTO ||
+	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+
+		// update guidance
+		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+
+		// calculate velocity, XXX should be airspeed, but using ground speed for now
+		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+
+		// altitude hold
+		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
+
+		// heading hold
+		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
+		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+		float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+		// velocity hold
+		// negative sign because nose over to increase speed
+		float thetaCmd = _theLimit.update(-_v2Theta.update(
+				_vLimit.update(_vCmd.get()) - v));
+		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+		// yaw rate cmd
+		float rCmd = 0;
+
+		// stabilization
+		_stabilization.update(pCmd, qCmd, rCmd,
+				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		// output
+		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+		_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+
+		// XXX limit throttle to manual setting (safety) for now.
+		// If it turns out to be confusing, it can be removed later once
+		// a first binary release can be targeted.
+		// This is not a hack, but a design choice.
+
+		/* do not limit in HIL */
+		if (!_status.flag_hil_enabled) {
+			/* limit to value of manual throttle */
+			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+						     _actuators.control[CH_THR] : _manual.throttle;
+		}
+
+	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+
+		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+			_actuators.control[CH_AIL] = _manual.roll;
+			_actuators.control[CH_ELV] = _manual.pitch;
+			_actuators.control[CH_RDR] = _manual.yaw;
+			_actuators.control[CH_THR] = _manual.throttle;
+
+		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+
+			// calculate velocity, XXX should be airspeed, but using ground speed for now
+			float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+
+			// pitch channel -> rate of climb
+			// TODO, might want to put a gain on this, otherwise commanding
+			// from +1 -> -1 m/s for rate of climb
+			//float dThrottle = _roc2Thr.update(
+			//_rocMax.get()*_manual.pitch - _pos.vz);
+
+			// roll channel -> bank angle
+			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+			float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+			// throttle channel -> velocity
+			// negative sign because nose over to increase speed
+			float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
+			float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+			// yaw rate cmd
+			float rCmd = 0;
+
+			// stabilization
+			_stabilization.update(pCmd, qCmd, rCmd,
+					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+			// output
+			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+			// currenlty using manual throttle
+			// XXX if you enable this watch out, vz might be very noisy
+			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+			_actuators.control[CH_THR] = _manual.throttle;
+
+			// XXX limit throttle to manual setting (safety) for now.
+			// If it turns out to be confusing, it can be removed later once
+			// a first binary release can be targeted.
+			// This is not a hack, but a design choice.
+
+			/* do not limit in HIL */
+			if (!_status.flag_hil_enabled) {
+				/* limit to value of manual throttle */
+				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							     _actuators.control[CH_THR] : _manual.throttle;
+			}
+		}
+
+		// body rates controller, disabled for now
+		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+
+			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
+					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+			_actuators.control[CH_AIL] = _stabilization.getAileron();
+			_actuators.control[CH_ELV] = _stabilization.getElevator();
+			_actuators.control[CH_RDR] = _stabilization.getRudder();
+			_actuators.control[CH_THR] = _manual.throttle;
+		}
+	}
+
+	// update all publications
+	updatePublications();
+}
+
+BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
+{
+	// send one last publication when destroyed, setting
+	// all output to zero
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	updatePublications();
+}
+
+} // namespace fixedwing
+
+} // namespace control
+

src/modules/controllib/fixedwing.hpp

@@ -0,0 +1,342 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 fixedwing.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+
+#include "blocks.hpp"
+#include "block/UOrbSubscription.hpp"
+#include "block/UOrbPublication.hpp"
+
+extern "C" {
+#include <systemlib/geo/geo.h>
+}
+
+namespace control
+{
+
+namespace fixedwing
+{
+
+/**
+ * BlockYawDamper
+ *
+ * This block has more explations to help new developers
+ * add their own blocks. It includes a limited explanation
+ * of some C++ basics.
+ *
+ * Block: The generic class describing a typical block as you
+ * would expect in Simulink or ScicosLab. A block can have
+ * parameters. It cannot have other blocks.
+ *
+ * SuperBlock: A superblock is a block that can have other
+ * blocks. It has methods that manage the blocks beneath it.
+ *
+ * BlockYawDamper inherits from SuperBlock publically, this
+ * means that any public function in SuperBlock are public within
+ * BlockYawDamper and may be called from outside the
+ * class methods. Any protected function within block
+ * are private to the class and may not be called from
+ * outside this class. Protected should be preferred
+ * where possible to public as it is important to
+ * limit access to the bare minimum to prevent
+ * accidental errors.
+ */
+class __EXPORT BlockYawDamper : public SuperBlock
+{
+private:
+	/**
+	 * Declaring other blocks used by this block
+	 *
+	 * In this section we declare all child blocks that
+	 * this block is composed of. They are private
+	 * members so only this block has direct access to
+	 * them.
+	 */
+	BlockLowPass _rLowPass;
+	BlockHighPass _rWashout;
+	BlockP _r2Rdr;
+
+	/**
+	 * Declaring output values and accessors
+	 *
+	 * If we have any output values for the block we
+	 * declare them here. Output can be directly returned
+	 * through the update function, but outputs should be
+	 * declared here if the information will likely be requested
+	 * again, or if there are multiple outputs.
+	 *
+	 * You should only be able to set outputs from this block,
+	 * so the outputs are declared in the private section.
+	 * A get accessor is provided
+	 * in the public section for other blocks to get the
+	 * value of the output.
+	 */
+	float _rudder;
+public:
+	/**
+	 * BlockYawDamper Constructor
+	 *
+	 * The job of the constructor is to initialize all
+	 * parameter in this block and initialize all child
+	 * blocks. Note also, that the output values must be
+	 * initialized here. The order of the members in the
+	 * member initialization list should follow the
+	 * order in which they are declared within the class.
+	 * See the private member declarations above.
+	 *
+	 * Block Construction
+	 *
+	 * All blocks are constructed with their parent block
+	 * and their name. This allows parameters within the
+	 * block to construct a fully qualified name from
+	 * concatenating the two. If the name provided to the
+	 * block is "", then the block will use the parent
+	 * name as it's name. This is useful in cases where
+	 * you have a block that has parameters "MIN", "MAX",
+	 * such as BlockLimit and you do not want an extra name
+	 * to qualify them since the parent block has no "MIN",
+	 * "MAX" parameters.
+	 *
+	 * Block Parameter Construction
+	 *
+	 * Block parameters are named constants, they are
+	 * constructed using:
+	 * BlockParam::BlockParam(Block * parent, const char * name)
+	 * This funciton takes both a parent block and a name.
+	 * The constructore then uses the parent name and the name of
+	 * the paramter to ask the px4 param library if it has any
+	 * parameters with this name. If it does, a handle to the
+	 * parameter is retrieved.
+	 *
+	 * Block/ BlockParam Naming
+	 *
+	 * When desigining new blocks, the naming of the parameters and
+	 * blocks determines the fully qualified name of the parameters
+	 * within the ground station, so it is important to choose
+	 * short, easily understandable names. Again, when a name of
+	 * "" is passed, the parent block name is used as the value to
+	 * prepend to paramters names.
+	 */
+	BlockYawDamper(SuperBlock *parent, const char *name);
+	/**
+	 * Block deconstructor
+	 *
+	 * It is always a good idea to declare a virtual
+	 * deconstructor so that upon calling delete from
+	 * a class derived from this, all of the
+	 * deconstructors all called, the derived class first, and
+	 * then the base class
+	 */
+	virtual ~BlockYawDamper();
+
+	/**
+	 * Block update function
+	 *
+	 * The job of the update function is to compute the output
+	 * values for the block. In a simple block with one output,
+	 * the output may be returned directly. If the output is
+	 * required frequenly by other processses, it might be a
+	 * good idea to declare a member to store the temporary
+	 * variable.
+	 */
+	void update(float rCmd, float r);
+
+	/**
+	 * Rudder output value accessor
+	 *
+	 * This is a public accessor function, which means that the
+	 * private value _rudder is returned to anyone calling
+	 * BlockYawDamper::getRudder(). Note thate a setRudder() is
+	 * not provided, this is because the updateParams() call
+	 * for a block provides the mechanism for updating the
+	 * paramter.
+	 */
+	float getRudder() { return _rudder; }
+};
+
+/**
+ * Stability augmentation system.
+ * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
+ */
+class __EXPORT BlockStabilization : public SuperBlock
+{
+private:
+	BlockYawDamper _yawDamper;
+	BlockLowPass _pLowPass;
+	BlockLowPass _qLowPass;
+	BlockP _p2Ail;
+	BlockP _q2Elv;
+	float _aileron;
+	float _elevator;
+public:
+	BlockStabilization(SuperBlock *parent, const char *name);
+	virtual ~BlockStabilization();
+	void update(float pCmd, float qCmd, float rCmd,
+		    float p, float q, float r);
+	float getAileron() { return _aileron; }
+	float getElevator() { return _elevator; }
+	float getRudder() { return _yawDamper.getRudder(); }
+};
+
+/**
+ * Frontside/ Backside Control Systems
+ *
+ * Frontside :
+ *   velocity error -> throttle
+ *   altitude error -> elevator
+ *
+ * Backside :
+ *   velocity error -> elevator
+ *   altitude error -> throttle
+ *
+ * Backside control systems are more resilient at
+ * slow speeds on the back-side of the power
+ * required curve/ landing etc. Less performance
+ * than frontside at high speeds.
+ */
+
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	virtual ~BlockWaypointGuidance();
+	void update(vehicle_global_position_s &pos,
+		    vehicle_attitude_s &att,
+		    vehicle_global_position_setpoint_s &posCmd,
+		    vehicle_global_position_setpoint_s &lastPosCmd);
+	float getPsiCmd() { return _psiCmd; }
+};
+
+/**
+ * UorbEnabledAutopilot
+ */
+class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
+{
+protected:
+	// subscriptions
+	UOrbSubscription<vehicle_attitude_s> _att;
+	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
+	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
+	UOrbSubscription<vehicle_global_position_s> _pos;
+	UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
+	UOrbSubscription<manual_control_setpoint_s> _manual;
+	UOrbSubscription<vehicle_status_s> _status;
+	UOrbSubscription<parameter_update_s> _param_update;
+	// publications
+	UOrbPublication<actuator_controls_s> _actuators;
+public:
+	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
+	virtual ~BlockUorbEnabledAutopilot();
+};
+
+/**
+ * Multi-mode Autopilot
+ */
+class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
+{
+private:
+	// stabilization
+	BlockStabilization _stabilization;
+
+	// heading hold
+	BlockP _psi2Phi;
+	BlockP _phi2P;
+	BlockLimitSym _phiLimit;
+
+	// velocity hold
+	BlockPID _v2Theta;
+	BlockPID _theta2Q;
+	BlockLimit _theLimit;
+	BlockLimit _vLimit;
+
+	// altitude/ roc hold
+	BlockPID _h2Thr;
+	BlockPID _roc2Thr;
+
+	// guidance
+	BlockWaypointGuidance _guide;
+
+	// block params
+	BlockParam<float> _trimAil;
+	BlockParam<float> _trimElv;
+	BlockParam<float> _trimRdr;
+	BlockParam<float> _trimThr;
+	BlockParam<float> _vCmd;
+	BlockParam<float> _rocMax;
+
+	struct pollfd _attPoll;
+	vehicle_global_position_setpoint_s _lastPosCmd;
+	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
+	uint64_t _timeStamp;
+public:
+	BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
+	void update();
+	virtual ~BlockMultiModeBacksideAutopilot();
+};
+
+
+} // namespace fixedwing
+
+} // namespace control
+

src/modules/controllib/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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
+# 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.
+#
+############################################################################
+
+#
+# Control library
+#
+SRCS		 = test_params.c \
+		   block/Block.cpp \
+		   block/BlockParam.cpp \
+		   block/UOrbPublication.cpp \
+		   block/UOrbSubscription.cpp \
+		   blocks.cpp \
+		   fixedwing.cpp

src/modules/controllib/test_params.c

@@ -0,0 +1,22 @@
+#include <systemlib/param/param.h>
+// WARNING:
+//  do not changes these unless
+//  you want to recompute the
+//  answers for all of the unit tests
+
+PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
+PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
+PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
+PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
+
+PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
+
+PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
+PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
+
+PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
+PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
+
+PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
+PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c

@@ -0,0 +1,159 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_abs_f32.c    
+*    
+* Description:	Vector absolute value.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+#include <math.h>
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @defgroup BasicAbs Vector Absolute Value        
+ *        
+ * Computes the absolute value of a vector on an element-by-element basis.        
+ *        
+ * <pre>        
+ *     pDst[n] = abs(pSrcA[n]),   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * The operation can be done in-place by setting the input and output pointers to the same buffer.        
+ * There are separate functions for floating-point, Q7, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup BasicAbs        
+ * @{        
+ */
+
+/**        
+ * @brief Floating-point vector absolute value.        
+ * @param[in]       *pSrc points to the input buffer        
+ * @param[out]      *pDst points to the output buffer        
+ * @param[in]       blockSize number of samples in each vector        
+ * @return none.        
+ */
+
+void arm_abs_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t in1, in2, in3, in4;                  /* temporary variables */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Calculate absolute and then store the results in the destination buffer. */
+    /* read sample from source */
+    in1 = *pSrc;
+    in2 = *(pSrc + 1);
+    in3 = *(pSrc + 2);
+
+    /* find absolute value */
+    in1 = fabsf(in1);
+
+    /* read sample from source */
+    in4 = *(pSrc + 3);
+
+    /* find absolute value */
+    in2 = fabsf(in2);
+
+    /* read sample from source */
+    *pDst = in1;
+
+    /* find absolute value */
+    in3 = fabsf(in3);
+
+    /* find absolute value */
+    in4 = fabsf(in4);
+
+    /* store result to destination */
+    *(pDst + 1) = in2;
+
+    /* store result to destination */
+    *(pDst + 2) = in3;
+
+    /* store result to destination */
+    *(pDst + 3) = in4;
+
+
+    /* Update source pointer to process next sampels */
+    pSrc += 4u;
+
+    /* Update destination pointer to process next sampels */
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /*   #ifndef ARM_MATH_CM0   */
+
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Calculate absolute and then store the results in the destination buffer. */
+    *pDst++ = fabsf(*pSrc++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of BasicAbs group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c

@@ -0,0 +1,173 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_abs_q15.c    
+*    
+* Description:	Q15 vector absolute value.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicAbs    
+ * @{    
+ */
+
+/**    
+ * @brief Q15 vector absolute value.    
+ * @param[in]       *pSrc points to the input buffer    
+ * @param[out]      *pDst points to the output buffer    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.    
+ */
+
+void arm_abs_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  q15_t in1;                                     /* Input value1 */
+  q15_t in2;                                     /* Input value2 */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Read two inputs */
+    in1 = *pSrc++;
+    in2 = *pSrc++;
+
+
+    /* Store the Absolute result in the destination buffer by packing the two values, in a single cycle */
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+    *__SIMD32(pDst)++ =
+      __PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
+              ((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
+
+#else
+
+
+    *__SIMD32(pDst)++ =
+      __PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
+              ((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
+
+#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+    in1 = *pSrc++;
+    in2 = *pSrc++;
+
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+    *__SIMD32(pDst)++ =
+      __PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
+              ((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
+
+#else
+
+
+    *__SIMD32(pDst)++ =
+      __PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
+              ((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
+
+#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Read the input */
+    in1 = *pSrc++;
+
+    /* Calculate absolute value of input and then store the result in the destination buffer. */
+    *pDst++ = (in1 > 0) ? in1 : __QSUB16(0, in1);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  q15_t in;                                      /* Temporary input variable */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Read the input */
+    in = *pSrc++;
+
+    /* Calculate absolute value of input and then store the result in the destination buffer. */
+    *pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of BasicAbs group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c

@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_abs_q31.c    
+*    
+* Description:	Q31 vector absolute value.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicAbs    
+ * @{    
+ */
+
+
+/**    
+ * @brief Q31 vector absolute value.    
+ * @param[in]       *pSrc points to the input buffer    
+ * @param[out]      *pDst points to the output buffer    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.    
+ */
+
+void arm_abs_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  q31_t in;                                      /* Input value */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t in1, in2, in3, in4;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Calculate absolute of input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
+    in1 = *pSrc++;
+    in2 = *pSrc++;
+    in3 = *pSrc++;
+    in4 = *pSrc++;
+
+    *pDst++ = (in1 > 0) ? in1 : __QSUB(0, in1);
+    *pDst++ = (in2 > 0) ? in2 : __QSUB(0, in2);
+    *pDst++ = (in3 > 0) ? in3 : __QSUB(0, in3);
+    *pDst++ = (in4 > 0) ? in4 : __QSUB(0, in4);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /*   #ifndef ARM_MATH_CM0   */
+
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Calculate absolute value of the input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
+    in = *pSrc++;
+    *pDst++ = (in > 0) ? in : ((in == 0x80000000) ? 0x7fffffff : -in);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+}
+
+/**    
+ * @} end of BasicAbs group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c

@@ -0,0 +1,152 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_abs_q7.c    
+*    
+* Description:	Q7 vector absolute value.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @addtogroup BasicAbs        
+ * @{        
+ */
+
+/**        
+ * @brief Q7 vector absolute value.        
+ * @param[in]       *pSrc points to the input buffer        
+ * @param[out]      *pDst points to the output buffer        
+ * @param[in]       blockSize number of samples in each vector        
+ * @return none.        
+ *    
+ * \par Conditions for optimum performance    
+ *  Input and output buffers should be aligned by 32-bit    
+ *    
+ *        
+ * <b>Scaling and Overflow Behavior:</b>        
+ * \par        
+ * The function uses saturating arithmetic.        
+ * The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F.        
+ */
+
+void arm_abs_q7(
+  q7_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  q7_t in;                                       /* Input value1 */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t in1, in2, in3, in4;                      /* temporary input variables */
+  q31_t out1, out2, out3, out4;                  /* temporary output variables */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Read inputs */
+    in1 = (q31_t) * pSrc;
+    in2 = (q31_t) * (pSrc + 1);
+    in3 = (q31_t) * (pSrc + 2);
+
+    /* find absolute value */
+    out1 = (in1 > 0) ? in1 : __QSUB8(0, in1);
+
+    /* read input */
+    in4 = (q31_t) * (pSrc + 3);
+
+    /* find absolute value */
+    out2 = (in2 > 0) ? in2 : __QSUB8(0, in2);
+
+    /* store result to destination */
+    *pDst = (q7_t) out1;
+
+    /* find absolute value */
+    out3 = (in3 > 0) ? in3 : __QSUB8(0, in3);
+
+    /* find absolute value */
+    out4 = (in4 > 0) ? in4 : __QSUB8(0, in4);
+
+    /* store result to destination */
+    *(pDst + 1) = (q7_t) out2;
+
+    /* store result to destination */
+    *(pDst + 2) = (q7_t) out3;
+
+    /* store result to destination */
+    *(pDst + 3) = (q7_t) out4;
+
+    /* update pointers to process next samples */
+    pSrc += 4u;
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+#else
+
+  /* Run the below code for Cortex-M0 */
+  blkCnt = blockSize;
+
+#endif //      #define ARM_MATH_CM0
+
+  while(blkCnt > 0u)
+  {
+    /* C = |A| */
+    /* Read the input */
+    in = *pSrc++;
+
+    /* Store the Absolute result in the destination buffer */
+    *pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? 0x7f : -in);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of BasicAbs group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_add_f32.c    
+*    
+* Description:	Floating-point vector addition.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @defgroup BasicAdd Vector Addition        
+ *        
+ * Element-by-element addition of two vectors.        
+ *        
+ * <pre>        
+ *     pDst[n] = pSrcA[n] + pSrcB[n],   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q7, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup BasicAdd        
+ * @{        
+ */
+
+/**        
+ * @brief Floating-point vector addition.        
+ * @param[in]       *pSrcA points to the first input vector        
+ * @param[in]       *pSrcB points to the second input vector        
+ * @param[out]      *pDst points to the output vector        
+ * @param[in]       blockSize number of samples in each vector        
+ * @return none.        
+ */
+
+void arm_add_f32(
+  float32_t * pSrcA,
+  float32_t * pSrcB,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t inA1, inA2, inA3, inA4;              /* temporary input variabels */
+  float32_t inB1, inB2, inB3, inB4;              /* temporary input variables */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+
+    /* read four inputs from sourceA and four inputs from sourceB */
+    inA1 = *pSrcA;
+    inB1 = *pSrcB;
+    inA2 = *(pSrcA + 1);
+    inB2 = *(pSrcB + 1);
+    inA3 = *(pSrcA + 2);
+    inB3 = *(pSrcB + 2);
+    inA4 = *(pSrcA + 3);
+    inB4 = *(pSrcB + 3);
+
+    /* C = A + B */
+    /* add and store result to destination */
+    *pDst = inA1 + inB1;
+    *(pDst + 1) = inA2 + inB2;
+    *(pDst + 2) = inA3 + inB3;
+    *(pDst + 3) = inA4 + inB4;
+
+    /* update pointers to process next samples */
+    pSrcA += 4u;
+    pSrcB += 4u;
+    pDst += 4u;
+
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *pDst++ = (*pSrcA++) + (*pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of BasicAdd group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c

@@ -0,0 +1,135 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_add_q15.c    
+*    
+* Description:	Q15 vector addition    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicAdd    
+ * @{    
+ */
+
+/**    
+ * @brief Q15 vector addition.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.    
+ */
+
+void arm_add_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inA1, inA2, inB1, inB2;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    inA1 = *__SIMD32(pSrcA)++;
+    inA2 = *__SIMD32(pSrcA)++;
+    inB1 = *__SIMD32(pSrcB)++;
+    inB2 = *__SIMD32(pSrcB)++;
+
+    *__SIMD32(pDst)++ = __QADD16(inA1, inB1);
+    *__SIMD32(pDst)++ = __QADD16(inA2, inB2);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *pDst++ = (q15_t) __QADD16(*pSrcA++, *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *pDst++ = (q15_t) __SSAT(((q31_t) * pSrcA++ + *pSrcB++), 16);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+}
+
+/**    
+ * @} end of BasicAdd group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c

@@ -0,0 +1,143 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_add_q31.c    
+*    
+* Description:	Q31 vector addition.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicAdd    
+ * @{    
+ */
+
+
+/**    
+ * @brief Q31 vector addition.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] will be saturated.    
+ */
+
+void arm_add_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inA1, inA2, inA3, inA4;
+  q31_t inB1, inB2, inB3, inB4;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    inA1 = *pSrcA++;
+    inA2 = *pSrcA++;
+    inB1 = *pSrcB++;
+    inB2 = *pSrcB++;
+
+    inA3 = *pSrcA++;
+    inA4 = *pSrcA++;
+    inB3 = *pSrcB++;
+    inB4 = *pSrcB++;
+
+    *pDst++ = __QADD(inA1, inB1);
+    *pDst++ = __QADD(inA2, inB2);
+    *pDst++ = __QADD(inA3, inB3);
+    *pDst++ = __QADD(inA4, inB4);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *pDst++ = __QADD(*pSrcA++, *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrcA++ + *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of BasicAdd group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c

@@ -0,0 +1,129 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_add_q7.c    
+*    
+* Description:	Q7 vector addition.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicAdd    
+ * @{    
+ */
+
+/**    
+ * @brief Q7 vector addition.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.    
+ */
+
+void arm_add_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *pDst++ = (q7_t) __SSAT(*pSrcA++ + *pSrcB++, 8);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + B */
+    /* Add and then store the results in the destination buffer. */
+    *pDst++ = (q7_t) __SSAT((q15_t) * pSrcA++ + *pSrcB++, 8);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+}
+
+/**    
+ * @} end of BasicAdd group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c

@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_dot_prod_f32.c    
+*    
+* Description:	Floating-point dot product.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @defgroup dot_prod Vector Dot Product    
+ *    
+ * Computes the dot product of two vectors.    
+ * The vectors are multiplied element-by-element and then summed.    
+ * There are separate functions for floating-point, Q7, Q15, and Q31 data types.    
+ */
+
+/**    
+ * @addtogroup dot_prod    
+ * @{    
+ */
+
+/**    
+ * @brief Dot product of floating-point vectors.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @param[out]      *result output result returned here    
+ * @return none.    
+ */
+
+
+void arm_dot_prod_f32(
+  float32_t * pSrcA,
+  float32_t * pSrcB,
+  uint32_t blockSize,
+  float32_t * result)
+{
+  float32_t sum = 0.0f;                          /* Temporary result storage */
+  uint32_t blkCnt;                               /* loop counter */
+
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Calculate dot product and then store the result in a temporary buffer */
+    sum += (*pSrcA++) * (*pSrcB++);
+    sum += (*pSrcA++) * (*pSrcB++);
+    sum += (*pSrcA++) * (*pSrcB++);
+    sum += (*pSrcA++) * (*pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Calculate dot product and then store the result in a temporary buffer. */
+    sum += (*pSrcA++) * (*pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+  /* Store the result back in the destination buffer */
+  *result = sum;
+}
+
+/**    
+ * @} end of dot_prod group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c

@@ -0,0 +1,135 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_dot_prod_q15.c    
+*    
+* Description:	Q15 dot product.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup dot_prod    
+ * @{    
+ */
+
+/**    
+ * @brief Dot product of Q15 vectors.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @param[out]      *result output result returned here    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The intermediate multiplications are in 1.15 x 1.15 = 2.30 format and these    
+ * results are added to a 64-bit accumulator in 34.30 format.    
+ * Nonsaturating additions are used and given that there are 33 guard bits in the accumulator    
+ * there is no risk of overflow.    
+ * The return result is in 34.30 format.    
+ */
+
+void arm_dot_prod_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  uint32_t blockSize,
+  q63_t * result)
+{
+  q63_t sum = 0;                                 /* Temporary result storage */
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Calculate dot product and then store the result in a temporary buffer. */
+    sum = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
+    sum = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Calculate dot product and then store the results in a temporary buffer. */
+    sum = __SMLALD(*pSrcA++, *pSrcB++, sum);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Calculate dot product and then store the results in a temporary buffer. */
+    sum += (q63_t) ((q31_t) * pSrcA++ * *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  /* Store the result in the destination buffer in 34.30 format */
+  *result = sum;
+
+}
+
+/**    
+ * @} end of dot_prod group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c

@@ -0,0 +1,138 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_dot_prod_q31.c    
+*    
+* Description:	Q31 dot product.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup dot_prod    
+ * @{    
+ */
+
+/**    
+ * @brief Dot product of Q31 vectors.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @param[out]      *result output result returned here    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The intermediate multiplications are in 1.31 x 1.31 = 2.62 format and these    
+ * are truncated to 2.48 format by discarding the lower 14 bits.    
+ * The 2.48 result is then added without saturation to a 64-bit accumulator in 16.48 format.    
+ * There are 15 guard bits in the accumulator and there is no risk of overflow as long as    
+ * the length of the vectors is less than 2^16 elements.    
+ * The return result is in 16.48 format.    
+ */
+
+void arm_dot_prod_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  uint32_t blockSize,
+  q63_t * result)
+{
+  q63_t sum = 0;                                 /* Temporary result storage */
+  uint32_t blkCnt;                               /* loop counter */
+
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inA1, inA2, inA3, inA4;
+  q31_t inB1, inB2, inB3, inB4;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Calculate dot product and then store the result in a temporary buffer. */
+    inA1 = *pSrcA++;
+    inA2 = *pSrcA++;
+    inA3 = *pSrcA++;
+    inA4 = *pSrcA++;
+    inB1 = *pSrcB++;
+    inB2 = *pSrcB++;
+    inB3 = *pSrcB++;
+    inB4 = *pSrcB++;
+
+    sum += ((q63_t) inA1 * inB1) >> 14u;
+    sum += ((q63_t) inA2 * inB2) >> 14u;
+    sum += ((q63_t) inA3 * inB3) >> 14u;
+    sum += ((q63_t) inA4 * inB4) >> 14u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Calculate dot product and then store the result in a temporary buffer. */
+    sum += ((q63_t) * pSrcA++ * *pSrcB++) >> 14u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* Store the result in the destination buffer in 16.48 format */
+  *result = sum;
+}
+
+/**    
+ * @} end of dot_prod group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c

@@ -0,0 +1,154 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_dot_prod_q7.c    
+*    
+* Description:	Q7 dot product.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup dot_prod    
+ * @{    
+ */
+
+/**    
+ * @brief Dot product of Q7 vectors.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @param[out]      *result output result returned here    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The intermediate multiplications are in 1.7 x 1.7 = 2.14 format and these    
+ * results are added to an accumulator in 18.14 format.    
+ * Nonsaturating additions are used and there is no danger of wrap around as long as    
+ * the vectors are less than 2^18 elements long.    
+ * The return result is in 18.14 format.    
+ */
+
+void arm_dot_prod_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  uint32_t blockSize,
+  q31_t * result)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+  q31_t sum = 0;                                 /* Temporary variables to store output */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  q31_t input1, input2;                          /* Temporary variables to store input */
+  q31_t inA1, inA2, inB1, inB2;                  /* Temporary variables to store input */
+
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* read 4 samples at a time from sourceA */
+    input1 = *__SIMD32(pSrcA)++;
+    /* read 4 samples at a time from sourceB */
+    input2 = *__SIMD32(pSrcB)++;
+
+    /* extract two q7_t samples to q15_t samples */
+    inA1 = __SXTB16(__ROR(input1, 8));
+    /* extract reminaing two samples */
+    inA2 = __SXTB16(input1);
+    /* extract two q7_t samples to q15_t samples */
+    inB1 = __SXTB16(__ROR(input2, 8));
+    /* extract reminaing two samples */
+    inB2 = __SXTB16(input2);
+
+    /* multiply and accumulate two samples at a time */
+    sum = __SMLAD(inA1, inB1, sum);
+    sum = __SMLAD(inA2, inB2, sum);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Dot product and then store the results in a temporary buffer. */
+    sum = __SMLAD(*pSrcA++, *pSrcB++, sum);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
+    /* Dot product and then store the results in a temporary buffer. */
+    sum += (q31_t) ((q15_t) * pSrcA++ * *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+  /* Store the result in the destination buffer in 18.14 format */
+  *result = sum;
+}
+
+/**    
+ * @} end of dot_prod group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c

@@ -0,0 +1,172 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_mult_f32.c    
+*    
+* Description:	Floating-point vector multiplication.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.5  2010/04/26     
+*    incorporated review comments and updated with latest CMSIS layer    
+*    
+* Version 0.0.3  2010/03/10     
+*    Initial version    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @defgroup BasicMult Vector Multiplication        
+ *        
+ * Element-by-element multiplication of two vectors.        
+ *        
+ * <pre>        
+ *     pDst[n] = pSrcA[n] * pSrcB[n],   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q7, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup BasicMult        
+ * @{        
+ */
+
+/**        
+ * @brief Floating-point vector multiplication.        
+ * @param[in]       *pSrcA points to the first input vector        
+ * @param[in]       *pSrcB points to the second input vector        
+ * @param[out]      *pDst points to the output vector        
+ * @param[in]       blockSize number of samples in each vector        
+ * @return none.        
+ */
+
+void arm_mult_f32(
+  float32_t * pSrcA,
+  float32_t * pSrcB,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counters */
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t inA1, inA2, inA3, inA4;              /* temporary input variables */
+  float32_t inB1, inB2, inB3, inB4;              /* temporary input variables */
+  float32_t out1, out2, out3, out4;              /* temporary output variables */
+
+  /* loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A * B */
+    /* Multiply the inputs and store the results in output buffer */
+    /* read sample from sourceA */
+    inA1 = *pSrcA;
+    /* read sample from sourceB */
+    inB1 = *pSrcB;
+    /* read sample from sourceA */
+    inA2 = *(pSrcA + 1);
+    /* read sample from sourceB */
+    inB2 = *(pSrcB + 1);
+
+    /* out = sourceA * sourceB */
+    out1 = inA1 * inB1;
+
+    /* read sample from sourceA */
+    inA3 = *(pSrcA + 2);
+    /* read sample from sourceB */
+    inB3 = *(pSrcB + 2);
+
+    /* out = sourceA * sourceB */
+    out2 = inA2 * inB2;
+
+    /* read sample from sourceA */
+    inA4 = *(pSrcA + 3);
+
+    /* store result to destination buffer */
+    *pDst = out1;
+
+    /* read sample from sourceB */
+    inB4 = *(pSrcB + 3);
+
+    /* out = sourceA * sourceB */
+    out3 = inA3 * inB3;
+
+    /* store result to destination buffer */
+    *(pDst + 1) = out2;
+
+    /* out = sourceA * sourceB */
+    out4 = inA4 * inB4;
+    /* store result to destination buffer */
+    *(pDst + 2) = out3;
+    /* store result to destination buffer */
+    *(pDst + 3) = out4;
+
+
+    /* update pointers to process next samples */
+    pSrcA += 4u;
+    pSrcB += 4u;
+    pDst += 4u;
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * B */
+    /* Multiply the inputs and store the results in output buffer */
+    *pDst++ = (*pSrcA++) * (*pSrcB++);
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of BasicMult group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c

@@ -0,0 +1,152 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_mult_q15.c    
+*    
+* Description:	Q15 vector multiplication.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.5  2010/04/26     
+*    incorporated review comments and updated with latest CMSIS layer    
+*    
+* Version 0.0.3  2010/03/10     
+*    Initial version    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicMult    
+ * @{    
+ */
+
+
+/**    
+ * @brief           Q15 vector multiplication    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.    
+ */
+
+void arm_mult_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counters */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inA1, inA2, inB1, inB2;                  /* temporary input variables */
+  q15_t out1, out2, out3, out4;                  /* temporary output variables */
+  q31_t mul1, mul2, mul3, mul4;                  /* temporary variables */
+
+  /* loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* read two samples at a time from sourceA */
+    inA1 = *__SIMD32(pSrcA)++;
+    /* read two samples at a time from sourceB */
+    inB1 = *__SIMD32(pSrcB)++;
+    /* read two samples at a time from sourceA */
+    inA2 = *__SIMD32(pSrcA)++;
+    /* read two samples at a time from sourceB */
+    inB2 = *__SIMD32(pSrcB)++;
+
+    /* multiply mul = sourceA * sourceB */
+    mul1 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
+    mul2 = (q31_t) ((q15_t) inA1 * (q15_t) inB1);
+    mul3 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB2 >> 16));
+    mul4 = (q31_t) ((q15_t) inA2 * (q15_t) inB2);
+
+    /* saturate result to 16 bit */
+    out1 = (q15_t) __SSAT(mul1 >> 15, 16);
+    out2 = (q15_t) __SSAT(mul2 >> 15, 16);
+    out3 = (q15_t) __SSAT(mul3 >> 15, 16);
+    out4 = (q15_t) __SSAT(mul4 >> 15, 16);
+
+    /* store the result */
+#ifndef ARM_MATH_BIG_ENDIAN
+
+    *__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
+    *__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
+
+#else
+
+    *__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
+    *__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
+
+#endif //      #ifndef ARM_MATH_BIG_ENDIAN
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * B */
+    /* Multiply the inputs and store the result in the destination buffer */
+    *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+}
+
+/**    
+ * @} end of BasicMult group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c

@@ -0,0 +1,143 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_mult_q31.c    
+*    
+* Description:	Q31 vector multiplication.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.5  2010/04/26     
+*    incorporated review comments and updated with latest CMSIS layer    
+*    
+* Version 0.0.3  2010/03/10     
+*    Initial version    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicMult    
+ * @{    
+ */
+
+/**    
+ * @brief Q31 vector multiplication.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] will be saturated.    
+ */
+
+void arm_mult_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counters */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inA1, inA2, inA3, inA4;                  /* temporary input variables */
+  q31_t inB1, inB2, inB3, inB4;                  /* temporary input variables */
+  q31_t out1, out2, out3, out4;                  /* temporary output variables */
+
+  /* loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A * B */
+    /* Multiply the inputs and then store the results in the destination buffer. */
+    inA1 = *pSrcA++;
+    inA2 = *pSrcA++;
+    inA3 = *pSrcA++;
+    inA4 = *pSrcA++;
+    inB1 = *pSrcB++;
+    inB2 = *pSrcB++;
+    inB3 = *pSrcB++;
+    inB4 = *pSrcB++;
+
+    out1 = ((q63_t) inA1 * inB1) >> 32;
+    out2 = ((q63_t) inA2 * inB2) >> 32;
+    out3 = ((q63_t) inA3 * inB3) >> 32;
+    out4 = ((q63_t) inA4 * inB4) >> 32;
+
+    out1 = __SSAT(out1, 31);
+    out2 = __SSAT(out2, 31);
+    out3 = __SSAT(out3, 31);
+    out4 = __SSAT(out4, 31);
+
+    *pDst++ = out1 << 1u;
+    *pDst++ = out2 << 1u;
+    *pDst++ = out3 << 1u;
+    *pDst++ = out4 << 1u;
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * B */
+    /* Multiply the inputs and then store the results in the destination buffer. */
+    *pDst++ =
+      (q31_t) clip_q63_to_q31(((q63_t) (*pSrcA++) * (*pSrcB++)) >> 31);
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+}
+
+/**    
+ * @} end of BasicMult group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c

@@ -0,0 +1,128 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_mult_q7.c    
+*    
+* Description:	Q7 vector multiplication.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+*    
+* Version 0.0.5  2010/04/26     
+*    incorporated review comments and updated with latest CMSIS layer    
+*    
+* Version 0.0.3  2010/03/10 DP    
+*    Initial version    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicMult    
+ * @{    
+ */
+
+/**    
+ * @brief           Q7 vector multiplication    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.    
+ */
+
+void arm_mult_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counters */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q7_t out1, out2, out3, out4;                   /* Temporary variables to store the product */
+
+  /* loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A * B */
+    /* Multiply the inputs and store the results in temporary variables */
+    out1 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
+    out2 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
+    out3 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
+    out4 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
+
+    /* Store the results of 4 inputs in the destination buffer in single cycle by packing */
+    *__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * B */
+    /* Multiply the inputs and store the result in the destination buffer */
+    *pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+}
+
+/**    
+ * @} end of BasicMult group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c

@@ -0,0 +1,137 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_negate_f32.c    
+*    
+* Description:	Negates floating-point vectors.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @defgroup negate Vector Negate        
+ *        
+ * Negates the elements of a vector.        
+ *        
+ * <pre>        
+ *     pDst[n] = -pSrc[n],   0 <= n < blockSize.        
+ * </pre>        
+ */
+
+/**        
+ * @addtogroup negate        
+ * @{        
+ */
+
+/**        
+ * @brief  Negates the elements of a floating-point vector.        
+ * @param[in]  *pSrc points to the input vector        
+ * @param[out]  *pDst points to the output vector        
+ * @param[in]  blockSize number of samples in the vector        
+ * @return none.        
+ */
+
+void arm_negate_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t in1, in2, in3, in4;                  /* temporary variables */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* read inputs from source */
+    in1 = *pSrc;
+    in2 = *(pSrc + 1);
+    in3 = *(pSrc + 2);
+    in4 = *(pSrc + 3);
+
+    /* negate the input */
+    in1 = -in1;
+    in2 = -in2;
+    in3 = -in3;
+    in4 = -in4;
+
+    /* store the result to destination */
+    *pDst = in1;
+    *(pDst + 1) = in2;
+    *(pDst + 2) = in3;
+    *(pDst + 3) = in4;
+
+    /* update pointers to process next samples */
+    pSrc += 4u;
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = -A */
+    /* Negate and then store the results in the destination buffer. */
+    *pDst++ = -*pSrc++;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of negate group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c

@@ -0,0 +1,137 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_negate_q15.c    
+*    
+* Description:	Negates Q15 vectors.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @addtogroup negate        
+ * @{        
+ */
+
+/**        
+ * @brief  Negates the elements of a Q15 vector.        
+ * @param[in]  *pSrc points to the input vector        
+ * @param[out]  *pDst points to the output vector        
+ * @param[in]  blockSize number of samples in the vector        
+ * @return none.        
+ *    
+ * \par Conditions for optimum performance    
+ *  Input and output buffers should be aligned by 32-bit    
+ *    
+ *        
+ * <b>Scaling and Overflow Behavior:</b>        
+ * \par        
+ * The function uses saturating arithmetic.        
+ * The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.        
+ */
+
+void arm_negate_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  q15_t in;
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  q31_t in1, in2;                                /* Temporary variables */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = -A */
+    /* Read two inputs at a time */
+    in1 = _SIMD32_OFFSET(pSrc);
+    in2 = _SIMD32_OFFSET(pSrc + 2);
+
+    /* negate two samples at a time */
+    in1 = __QSUB16(0, in1);
+
+    /* negate two samples at a time */
+    in2 = __QSUB16(0, in2);
+
+    /* store the result to destination 2 samples at a time */
+    _SIMD32_OFFSET(pDst) = in1;
+    /* store the result to destination 2 samples at a time */
+    _SIMD32_OFFSET(pDst + 2) = in2;
+
+
+    /* update pointers to process next samples */
+    pSrc += 4u;
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = -A */
+    /* Negate and then store the result in the destination buffer. */
+    in = *pSrc++;
+    *pDst++ = (in == (q15_t) 0x8000) ? 0x7fff : -in;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of negate group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c

@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_negate_q31.c    
+*    
+* Description:	Negates Q31 vectors.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup negate    
+ * @{    
+ */
+
+/**    
+ * @brief  Negates the elements of a Q31 vector.    
+ * @param[in]  *pSrc points to the input vector    
+ * @param[out]  *pDst points to the output vector    
+ * @param[in]  blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.    
+ */
+
+void arm_negate_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  q31_t in;                                      /* Temporary variable */
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t in1, in2, in3, in4;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = -A */
+    /* Negate and then store the results in the destination buffer. */
+    in1 = *pSrc++;
+    in2 = *pSrc++;
+    in3 = *pSrc++;
+    in4 = *pSrc++;
+
+    *pDst++ = __QSUB(0, in1);
+    *pDst++ = __QSUB(0, in2);
+    *pDst++ = __QSUB(0, in3);
+    *pDst++ = __QSUB(0, in4);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+  while(blkCnt > 0u)
+  {
+    /* C = -A */
+    /* Negate and then store the result in the destination buffer. */
+    in = *pSrc++;
+    *pDst++ = (in == 0x80000000) ? 0x7fffffff : -in;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**    
+ * @} end of negate group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c

@@ -0,0 +1,120 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_negate_q7.c    
+*    
+* Description:	Negates Q7 vectors.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup negate    
+ * @{    
+ */
+
+/**    
+ * @brief  Negates the elements of a Q7 vector.    
+ * @param[in]  *pSrc points to the input vector    
+ * @param[out]  *pDst points to the output vector    
+ * @param[in]  blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F.    
+ */
+
+void arm_negate_q7(
+  q7_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  q7_t in;
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t input;                                   /* Input values1-4 */
+  q31_t zero = 0x00000000;
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = -A */
+    /* Read four inputs */
+    input = *__SIMD32(pSrc)++;
+
+    /* Store the Negated results in the destination buffer in a single cycle by packing the results */
+    *__SIMD32(pDst)++ = __QSUB8(zero, input);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = -A */
+    /* Negate and then store the results in the destination buffer. */ \
+      in = *pSrc++;
+    *pDst++ = (in == (q7_t) 0x80) ? 0x7f : -in;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**    
+ * @} end of negate group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c

@@ -0,0 +1,158 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_offset_f32.c    
+*    
+* Description:	Floating-point vector offset.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* ---------------------------------------------------------------------------- */
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @defgroup offset Vector Offset        
+ *        
+ * Adds a constant offset to each element of a vector.        
+ *        
+ * <pre>        
+ *     pDst[n] = pSrc[n] + offset,   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q7, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup offset        
+ * @{        
+ */
+
+/**        
+ * @brief  Adds a constant offset to a floating-point vector.        
+ * @param[in]  *pSrc points to the input vector        
+ * @param[in]  offset is the offset to be added        
+ * @param[out]  *pDst points to the output vector        
+ * @param[in]  blockSize number of samples in the vector        
+ * @return none.        
+ */
+
+
+void arm_offset_f32(
+  float32_t * pSrc,
+  float32_t offset,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t in1, in2, in3, in4;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the results in the destination buffer. */
+    /* read samples from source */
+    in1 = *pSrc;
+    in2 = *(pSrc + 1);
+
+    /* add offset to input */
+    in1 = in1 + offset;
+
+    /* read samples from source */
+    in3 = *(pSrc + 2);
+
+    /* add offset to input */
+    in2 = in2 + offset;
+
+    /* read samples from source */
+    in4 = *(pSrc + 3);
+
+    /* add offset to input */
+    in3 = in3 + offset;
+
+    /* store result to destination */
+    *pDst = in1;
+
+    /* add offset to input */
+    in4 = in4 + offset;
+
+    /* store result to destination */
+    *(pDst + 1) = in2;
+
+    /* store result to destination */
+    *(pDst + 2) = in3;
+
+    /* store result to destination */
+    *(pDst + 3) = in4;
+
+    /* update pointers to process next samples */
+    pSrc += 4u;
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the result in the destination buffer. */
+    *pDst++ = (*pSrc++) + offset;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of offset group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c

@@ -0,0 +1,131 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_offset_q15.c    
+*    
+* Description:	Q15 vector offset.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup offset    
+ * @{    
+ */
+
+/**    
+ * @brief  Adds a constant offset to a Q15 vector.    
+ * @param[in]  *pSrc points to the input vector    
+ * @param[in]  offset is the offset to be added    
+ * @param[out]  *pDst points to the output vector    
+ * @param[in]  blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated.    
+ */
+
+void arm_offset_q15(
+  q15_t * pSrc,
+  q15_t offset,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t offset_packed;                           /* Offset packed to 32 bit */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* Offset is packed to 32 bit in order to use SIMD32 for addition */
+  offset_packed = __PKHBT(offset, offset, 16);
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the results in the destination buffer, 2 samples at a time. */
+    *__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
+    *__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the results in the destination buffer. */
+    *pDst++ = (q15_t) __QADD16(*pSrc++, offset);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the results in the destination buffer. */
+    *pDst++ = (q15_t) __SSAT(((q31_t) * pSrc++ + offset), 16);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of offset group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c

@@ -0,0 +1,135 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_offset_q31.c    
+*    
+* Description:	Q31 vector offset.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup offset    
+ * @{    
+ */
+
+/**    
+ * @brief  Adds a constant offset to a Q31 vector.    
+ * @param[in]  *pSrc points to the input vector    
+ * @param[in]  offset is the offset to be added    
+ * @param[out]  *pDst points to the output vector    
+ * @param[in]  blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] are saturated.    
+ */
+
+void arm_offset_q31(
+  q31_t * pSrc,
+  q31_t offset,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t in1, in2, in3, in4;
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the results in the destination buffer. */
+    in1 = *pSrc++;
+    in2 = *pSrc++;
+    in3 = *pSrc++;
+    in4 = *pSrc++;
+
+    *pDst++ = __QADD(in1, offset);
+    *pDst++ = __QADD(in2, offset);
+    *pDst++ = __QADD(in3, offset);
+    *pDst++ = __QADD(in4, offset);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the result in the destination buffer. */
+    *pDst++ = __QADD(*pSrc++, offset);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the result in the destination buffer. */
+    *pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrc++ + offset);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of offset group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c

@@ -0,0 +1,130 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_offset_q7.c    
+*    
+* Description:	Q7 vector offset.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup offset    
+ * @{    
+ */
+
+/**    
+ * @brief  Adds a constant offset to a Q7 vector.    
+ * @param[in]  *pSrc points to the input vector    
+ * @param[in]  offset is the offset to be added    
+ * @param[out]  *pDst points to the output vector    
+ * @param[in]  blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q7 range [0x80 0x7F] are saturated.    
+ */
+
+void arm_offset_q7(
+  q7_t * pSrc,
+  q7_t offset,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t offset_packed;                           /* Offset packed to 32 bit */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* Offset is packed to 32 bit in order to use SIMD32 for addition */
+  offset_packed = __PACKq7(offset, offset, offset, offset);
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the results in the destination bufferfor 4 samples at a time. */
+    *__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrc)++, offset_packed);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the result in the destination buffer. */
+    *pDst++ = (q7_t) __SSAT(*pSrc++ + offset, 8);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A + offset */
+    /* Add offset and then store the result in the destination buffer. */
+    *pDst++ = (q7_t) __SSAT((q15_t) * pSrc++ + offset, 8);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of offset group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c

@@ -0,0 +1,161 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_scale_f32.c    
+*    
+* Description:	Multiplies a floating-point vector by a scalar.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @defgroup scale Vector Scale        
+ *        
+ * Multiply a vector by a scalar value.  For floating-point data, the algorithm used is:        
+ *        
+ * <pre>        
+ *     pDst[n] = pSrc[n] * scale,   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * In the fixed-point Q7, Q15, and Q31 functions, <code>scale</code> is represented by        
+ * a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>.        
+ * The shift allows the gain of the scaling operation to exceed 1.0.        
+ * The algorithm used with fixed-point data is:        
+ *        
+ * <pre>        
+ *     pDst[n] = (pSrc[n] * scaleFract) << shift,   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * The overall scale factor applied to the fixed-point data is        
+ * <pre>        
+ *     scale = scaleFract * 2^shift.        
+ * </pre>        
+ */
+
+/**        
+ * @addtogroup scale        
+ * @{        
+ */
+
+/**        
+ * @brief Multiplies a floating-point vector by a scalar.        
+ * @param[in]       *pSrc points to the input vector        
+ * @param[in]       scale scale factor to be applied        
+ * @param[out]      *pDst points to the output vector        
+ * @param[in]       blockSize number of samples in the vector        
+ * @return none.        
+ */
+
+
+void arm_scale_f32(
+  float32_t * pSrc,
+  float32_t scale,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t in1, in2, in3, in4;                  /* temporary variabels */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A * scale */
+    /* Scale the input and then store the results in the destination buffer. */
+    /* read input samples from source */
+    in1 = *pSrc;
+    in2 = *(pSrc + 1);
+
+    /* multiply with scaling factor */
+    in1 = in1 * scale;
+
+    /* read input sample from source */
+    in3 = *(pSrc + 2);
+
+    /* multiply with scaling factor */
+    in2 = in2 * scale;
+
+    /* read input sample from source */
+    in4 = *(pSrc + 3);
+
+    /* multiply with scaling factor */
+    in3 = in3 * scale;
+    in4 = in4 * scale;
+    /* store the result to destination */
+    *pDst = in1;
+    *(pDst + 1) = in2;
+    *(pDst + 2) = in3;
+    *(pDst + 3) = in4;
+
+    /* update pointers to process next samples */
+    pSrc += 4u;
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * scale */
+    /* Scale the input and then store the result in the destination buffer. */
+    *pDst++ = (*pSrc++) * scale;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of scale group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c

@@ -0,0 +1,157 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_scale_q15.c    
+*    
+* Description:	Multiplies a Q15 vector by a scalar.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup scale    
+ * @{    
+ */
+
+/**    
+ * @brief Multiplies a Q15 vector by a scalar.    
+ * @param[in]       *pSrc points to the input vector    
+ * @param[in]       scaleFract fractional portion of the scale value    
+ * @param[in]       shift number of bits to shift the result by    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.15 format.    
+ * These are multiplied to yield a 2.30 intermediate result and this is shifted with saturation to 1.15 format.    
+ */
+
+
+void arm_scale_q15(
+  q15_t * pSrc,
+  q15_t scaleFract,
+  int8_t shift,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  int8_t kShift = 15 - shift;                    /* shift to apply after scaling */
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q15_t in1, in2, in3, in4;
+  q31_t inA1, inA2;                              /* Temporary variables */
+  q31_t out1, out2, out3, out4;
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* Reading 2 inputs from memory */
+    inA1 = *__SIMD32(pSrc)++;
+    inA2 = *__SIMD32(pSrc)++;
+
+    /* C = A * scale */
+    /* Scale the inputs and then store the 2 results in the destination buffer        
+     * in single cycle by packing the outputs */
+    out1 = (q31_t) ((q15_t) (inA1 >> 16) * scaleFract);
+    out2 = (q31_t) ((q15_t) inA1 * scaleFract);
+    out3 = (q31_t) ((q15_t) (inA2 >> 16) * scaleFract);
+    out4 = (q31_t) ((q15_t) inA2 * scaleFract);
+
+    /* apply shifting */
+    out1 = out1 >> kShift;
+    out2 = out2 >> kShift;
+    out3 = out3 >> kShift;
+    out4 = out4 >> kShift;
+
+    /* saturate the output */
+    in1 = (q15_t) (__SSAT(out1, 16));
+    in2 = (q15_t) (__SSAT(out2, 16));
+    in3 = (q15_t) (__SSAT(out3, 16));
+    in4 = (q15_t) (__SSAT(out4, 16));
+
+    /* store the result to destination */
+    *__SIMD32(pDst)++ = __PKHBT(in2, in1, 16);
+    *__SIMD32(pDst)++ = __PKHBT(in4, in3, 16);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * scale */
+    /* Scale the input and then store the result in the destination buffer. */
+    *pDst++ = (q15_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 16));
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * scale */
+    /* Scale the input and then store the result in the destination buffer. */
+    *pDst++ = (q15_t) (__SSAT(((q31_t) * pSrc++ * scaleFract) >> kShift, 16));
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of scale group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c

@@ -0,0 +1,221 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_scale_q31.c    
+*    
+* Description:	Multiplies a Q31 vector by a scalar.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**       
+ * @ingroup groupMath       
+ */
+
+/**       
+ * @addtogroup scale       
+ * @{       
+ */
+
+/**       
+ * @brief Multiplies a Q31 vector by a scalar.       
+ * @param[in]       *pSrc points to the input vector       
+ * @param[in]       scaleFract fractional portion of the scale value       
+ * @param[in]       shift number of bits to shift the result by       
+ * @param[out]      *pDst points to the output vector       
+ * @param[in]       blockSize number of samples in the vector       
+ * @return none.       
+ *       
+ * <b>Scaling and Overflow Behavior:</b>       
+ * \par       
+ * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format.       
+ * These are multiplied to yield a 2.62 intermediate result and this is shifted with saturation to 1.31 format.       
+ */
+
+void arm_scale_q31(
+  q31_t * pSrc,
+  q31_t scaleFract,
+  int8_t shift,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  int8_t kShift = shift + 1;                     /* Shift to apply after scaling */
+  int8_t sign = (kShift & 0x80);
+  uint32_t blkCnt;                               /* loop counter */
+  q31_t in, out;
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  q31_t in1, in2, in3, in4;                      /* temporary input variables */
+  q31_t out1, out2, out3, out4;                  /* temporary output variabels */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  if(sign == 0u)
+  {
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.       
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* read four inputs from source */
+      in1 = *pSrc;
+      in2 = *(pSrc + 1);
+      in3 = *(pSrc + 2);
+      in4 = *(pSrc + 3);
+
+      /* multiply input with scaler value */
+      in1 = ((q63_t) in1 * scaleFract) >> 32;
+      in2 = ((q63_t) in2 * scaleFract) >> 32;
+      in3 = ((q63_t) in3 * scaleFract) >> 32;
+      in4 = ((q63_t) in4 * scaleFract) >> 32;
+
+      /* apply shifting */
+      out1 = in1 << kShift;
+      out2 = in2 << kShift;
+
+      /* saturate the results. */
+      if(in1 != (out1 >> kShift))
+        out1 = 0x7FFFFFFF ^ (in1 >> 31);
+
+      if(in2 != (out2 >> kShift))
+        out2 = 0x7FFFFFFF ^ (in2 >> 31);
+
+      out3 = in3 << kShift;
+      out4 = in4 << kShift;
+
+      *pDst = out1;
+      *(pDst + 1) = out2;
+
+      if(in3 != (out3 >> kShift))
+        out3 = 0x7FFFFFFF ^ (in3 >> 31);
+
+      if(in4 != (out4 >> kShift))
+        out4 = 0x7FFFFFFF ^ (in4 >> 31);
+
+      /* Store result destination */
+      *(pDst + 2) = out3;
+      *(pDst + 3) = out4;
+
+      /* Update pointers to process next sampels */
+      pSrc += 4u;
+      pDst += 4u;
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+
+  }
+  else
+  {
+    kShift = -kShift;
+
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.       
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* read four inputs from source */
+      in1 = *pSrc;
+      in2 = *(pSrc + 1);
+      in3 = *(pSrc + 2);
+      in4 = *(pSrc + 3);
+
+      /* multiply input with scaler value */
+      in1 = ((q63_t) in1 * scaleFract) >> 32;
+      in2 = ((q63_t) in2 * scaleFract) >> 32;
+      in3 = ((q63_t) in3 * scaleFract) >> 32;
+      in4 = ((q63_t) in4 * scaleFract) >> 32;
+
+      /* apply shifting */
+      out1 = in1 >> kShift;
+      out2 = in2 >> kShift;
+
+      out3 = in3 >> kShift;
+      out4 = in4 >> kShift;
+
+      /* Store result destination */
+      *pDst = out1;
+      *(pDst + 1) = out2;
+
+      *(pDst + 2) = out3;
+      *(pDst + 3) = out4;
+
+      /* Update pointers to process next sampels */
+      pSrc += 4u;
+      pDst += 4u;
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.       
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * scale */
+    /* Scale the input and then store the result in the destination buffer. */
+    in = *pSrc++;
+    in = ((q63_t) in * scaleFract) >> 32;
+
+    if(sign == 0)
+    {
+      out = in << kShift;
+      if(in != (out >> kShift))
+        out = 0x7FFFFFFF ^ (in >> 31);
+    }
+    else
+    {
+      out = in >> kShift;
+    }
+
+    *pDst++ = out;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**       
+ * @} end of scale group       
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c

@@ -0,0 +1,144 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_scale_q7.c    
+*    
+* Description:	Multiplies a Q7 vector by a scalar.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup scale    
+ * @{    
+ */
+
+/**    
+ * @brief Multiplies a Q7 vector by a scalar.    
+ * @param[in]       *pSrc points to the input vector    
+ * @param[in]       scaleFract fractional portion of the scale value    
+ * @param[in]       shift number of bits to shift the result by    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.7 format.    
+ * These are multiplied to yield a 2.14 intermediate result and this is shifted with saturation to 1.7 format.    
+ */
+
+void arm_scale_q7(
+  q7_t * pSrc,
+  q7_t scaleFract,
+  int8_t shift,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  int8_t kShift = 7 - shift;                     /* shift to apply after scaling */
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q7_t in1, in2, in3, in4, out1, out2, out3, out4;      /* Temporary variables to store input & output */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* Reading 4 inputs from memory */
+    in1 = *pSrc++;
+    in2 = *pSrc++;
+    in3 = *pSrc++;
+    in4 = *pSrc++;
+
+    /* C = A * scale */
+    /* Scale the inputs and then store the results in the temporary variables. */
+    out1 = (q7_t) (__SSAT(((in1) * scaleFract) >> kShift, 8));
+    out2 = (q7_t) (__SSAT(((in2) * scaleFract) >> kShift, 8));
+    out3 = (q7_t) (__SSAT(((in3) * scaleFract) >> kShift, 8));
+    out4 = (q7_t) (__SSAT(((in4) * scaleFract) >> kShift, 8));
+
+    /* Packing the individual outputs into 32bit and storing in    
+     * destination buffer in single write */
+    *__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * scale */
+    /* Scale the input and then store the result in the destination buffer. */
+    *pDst++ = (q7_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 8));
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A * scale */
+    /* Scale the input and then store the result in the destination buffer. */
+    *pDst++ = (q7_t) (__SSAT((((q15_t) * pSrc++ * scaleFract) >> kShift), 8));
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of scale group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c

@@ -0,0 +1,243 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_shift_q15.c    
+*    
+* Description:	Shifts the elements of a Q15 vector by a specified number of bits.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup shift    
+ * @{    
+ */
+
+/**    
+ * @brief  Shifts the elements of a Q15 vector a specified number of bits.    
+ * @param[in]  *pSrc points to the input vector    
+ * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.    
+ * @param[out]  *pDst points to the output vector    
+ * @param[in]  blockSize number of samples in the vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.    
+ */
+
+void arm_shift_q15(
+  q15_t * pSrc,
+  int8_t shiftBits,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  uint8_t sign;                                  /* Sign of shiftBits */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  q15_t in1, in2;                                /* Temporary variables */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* Getting the sign of shiftBits */
+  sign = (shiftBits & 0x80);
+
+  /* If the shift value is positive then do right shift else left shift */
+  if(sign == 0u)
+  {
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* Read 2 inputs */
+      in1 = *pSrc++;
+      in2 = *pSrc++;
+      /* C = A << shiftBits */
+      /* Shift the inputs and then store the results in the destination buffer. */
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
+                                  __SSAT((in2 << shiftBits), 16), 16);
+
+#else
+
+      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
+                                  __SSAT((in1 << shiftBits), 16), 16);
+
+#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+      in1 = *pSrc++;
+      in2 = *pSrc++;
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
+                                  __SSAT((in2 << shiftBits), 16), 16);
+
+#else
+
+      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
+                                  __SSAT((in1 << shiftBits), 16), 16);
+
+#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+
+    /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+     ** No loop unrolling is used. */
+    blkCnt = blockSize % 0x4u;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A << shiftBits */
+      /* Shift and then store the results in the destination buffer. */
+      *pDst++ = __SSAT((*pSrc++ << shiftBits), 16);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+  else
+  {
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* Read 2 inputs */
+      in1 = *pSrc++;
+      in2 = *pSrc++;
+
+      /* C = A >> shiftBits */
+      /* Shift the inputs and then store the results in the destination buffer. */
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      *__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
+                                  (in2 >> -shiftBits), 16);
+
+#else
+
+      *__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
+                                  (in1 >> -shiftBits), 16);
+
+#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+      in1 = *pSrc++;
+      in2 = *pSrc++;
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      *__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
+                                  (in2 >> -shiftBits), 16);
+
+#else
+
+      *__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
+                                  (in1 >> -shiftBits), 16);
+
+#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+
+    /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+     ** No loop unrolling is used. */
+    blkCnt = blockSize % 0x4u;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A >> shiftBits */
+      /* Shift the inputs and then store the results in the destination buffer. */
+      *pDst++ = (*pSrc++ >> -shiftBits);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Getting the sign of shiftBits */
+  sign = (shiftBits & 0x80);
+
+  /* If the shift value is positive then do right shift else left shift */
+  if(sign == 0u)
+  {
+    /* Initialize blkCnt with number of samples */
+    blkCnt = blockSize;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A << shiftBits */
+      /* Shift and then store the results in the destination buffer. */
+      *pDst++ = __SSAT(((q31_t) * pSrc++ << shiftBits), 16);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+  else
+  {
+    /* Initialize blkCnt with number of samples */
+    blkCnt = blockSize;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A >> shiftBits */
+      /* Shift the inputs and then store the results in the destination buffer. */
+      *pDst++ = (*pSrc++ >> -shiftBits);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of shift group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c

@@ -0,0 +1,195 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_shift_q31.c    
+*    
+* Description:	Shifts the elements of a Q31 vector by a specified number of bits.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+/**        
+ * @defgroup shift Vector Shift        
+ *        
+ * Shifts the elements of a fixed-point vector by a specified number of bits.        
+ * There are separate functions for Q7, Q15, and Q31 data types.        
+ * The underlying algorithm used is:        
+ *        
+ * <pre>        
+ *     pDst[n] = pSrc[n] << shift,   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * If <code>shift</code> is positive then the elements of the vector are shifted to the left.        
+ * If <code>shift</code> is negative then the elements of the vector are shifted to the right.        
+ */
+
+/**        
+ * @addtogroup shift        
+ * @{        
+ */
+
+/**        
+ * @brief  Shifts the elements of a Q31 vector a specified number of bits.        
+ * @param[in]  *pSrc points to the input vector        
+ * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.        
+ * @param[out]  *pDst points to the output vector        
+ * @param[in]  blockSize number of samples in the vector        
+ * @return none.        
+ *        
+ *        
+ * <b>Scaling and Overflow Behavior:</b>        
+ * \par        
+ * The function uses saturating arithmetic.        
+ * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated.        
+ */
+
+void arm_shift_q31(
+  q31_t * pSrc,
+  int8_t shiftBits,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  uint8_t sign = (shiftBits & 0x80);             /* Sign of shiftBits */
+
+#ifndef ARM_MATH_CM0
+
+  q31_t in1, in2, in3, in4;                      /* Temporary input variables */
+  q31_t out1, out2, out3, out4;                  /* Temporary output variables */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+
+  if(sign == 0u)
+  {
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* C = A  << shiftBits */
+      /* Shift the input and then store the results in the destination buffer. */
+      in1 = *pSrc;
+      in2 = *(pSrc + 1);
+      out1 = in1 << shiftBits;
+      in3 = *(pSrc + 2);
+      out2 = in2 << shiftBits;
+      in4 = *(pSrc + 3);
+      if(in1 != (out1 >> shiftBits))
+        out1 = 0x7FFFFFFF ^ (in1 >> 31);
+
+      if(in2 != (out2 >> shiftBits))
+        out2 = 0x7FFFFFFF ^ (in2 >> 31);
+
+      *pDst = out1;
+      out3 = in3 << shiftBits;
+      *(pDst + 1) = out2;
+      out4 = in4 << shiftBits;
+
+      if(in3 != (out3 >> shiftBits))
+        out3 = 0x7FFFFFFF ^ (in3 >> 31);
+
+      if(in4 != (out4 >> shiftBits))
+        out4 = 0x7FFFFFFF ^ (in4 >> 31);
+
+      *(pDst + 2) = out3;
+      *(pDst + 3) = out4;
+
+      /* Update destination pointer to process next sampels */
+      pSrc += 4u;
+      pDst += 4u;
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+  else
+  {
+
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* C = A >>  shiftBits */
+      /* Shift the input and then store the results in the destination buffer. */
+      in1 = *pSrc;
+      in2 = *(pSrc + 1);
+      in3 = *(pSrc + 2);
+      in4 = *(pSrc + 3);
+
+      *pDst = (in1 >> -shiftBits);
+      *(pDst + 1) = (in2 >> -shiftBits);
+      *(pDst + 2) = (in3 >> -shiftBits);
+      *(pDst + 3) = (in4 >> -shiftBits);
+
+
+      pSrc += 4u;
+      pDst += 4u;
+
+      blkCnt--;
+    }
+
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+  while(blkCnt > 0u)
+  {
+    /* C = A (>> or <<) shiftBits */
+    /* Shift the input and then store the result in the destination buffer. */
+    *pDst++ = (sign == 0u) ? clip_q63_to_q31((q63_t) * pSrc++ << shiftBits) :
+      (*pSrc++ >> -shiftBits);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+
+}
+
+/**        
+ * @} end of shift group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c

@@ -0,0 +1,215 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_shift_q7.c    
+*    
+* Description:	Processing function for the Q7 Shifting    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @addtogroup shift        
+ * @{        
+ */
+
+
+/**        
+ * @brief  Shifts the elements of a Q7 vector a specified number of bits.        
+ * @param[in]  *pSrc points to the input vector        
+ * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.        
+ * @param[out]  *pDst points to the output vector        
+ * @param[in]  blockSize number of samples in the vector        
+ * @return none.        
+ *    
+ * \par Conditions for optimum performance    
+ *  Input and output buffers should be aligned by 32-bit    
+ *    
+ *        
+ * <b>Scaling and Overflow Behavior:</b>        
+ * \par        
+ * The function uses saturating arithmetic.        
+ * Results outside of the allowable Q7 range [0x8 0x7F] will be saturated.        
+ */
+
+void arm_shift_q7(
+  q7_t * pSrc,
+  int8_t shiftBits,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  uint8_t sign;                                  /* Sign of shiftBits */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q7_t in1;                                      /* Input value1 */
+  q7_t in2;                                      /* Input value2 */
+  q7_t in3;                                      /* Input value3 */
+  q7_t in4;                                      /* Input value4 */
+
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* Getting the sign of shiftBits */
+  sign = (shiftBits & 0x80);
+
+  /* If the shift value is positive then do right shift else left shift */
+  if(sign == 0u)
+  {
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* C = A << shiftBits */
+      /* Read 4 inputs */
+      in1 = *pSrc;
+      in2 = *(pSrc + 1);
+      in3 = *(pSrc + 2);
+      in4 = *(pSrc + 3);
+
+      /* Store the Shifted result in the destination buffer in single cycle by packing the outputs */
+      *__SIMD32(pDst)++ = __PACKq7(__SSAT((in1 << shiftBits), 8),
+                                   __SSAT((in2 << shiftBits), 8),
+                                   __SSAT((in3 << shiftBits), 8),
+                                   __SSAT((in4 << shiftBits), 8));
+      /* Update source pointer to process next sampels */
+      pSrc += 4u;
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+
+    /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+     ** No loop unrolling is used. */
+    blkCnt = blockSize % 0x4u;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A << shiftBits */
+      /* Shift the input and then store the result in the destination buffer. */
+      *pDst++ = (q7_t) __SSAT((*pSrc++ << shiftBits), 8);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+  else
+  {
+    shiftBits = -shiftBits;
+    /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+     ** a second loop below computes the remaining 1 to 3 samples. */
+    while(blkCnt > 0u)
+    {
+      /* C = A >> shiftBits */
+      /* Read 4 inputs */
+      in1 = *pSrc;
+      in2 = *(pSrc + 1);
+      in3 = *(pSrc + 2);
+      in4 = *(pSrc + 3);
+
+      /* Store the Shifted result in the destination buffer in single cycle by packing the outputs */
+      *__SIMD32(pDst)++ = __PACKq7((in1 >> shiftBits), (in2 >> shiftBits),
+                                   (in3 >> shiftBits), (in4 >> shiftBits));
+
+
+      pSrc += 4u;
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+
+    /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+     ** No loop unrolling is used. */
+    blkCnt = blockSize % 0x4u;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A >> shiftBits */
+      /* Shift the input and then store the result in the destination buffer. */
+      in1 = *pSrc++;
+      *pDst++ = (in1 >> shiftBits);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Getting the sign of shiftBits */
+  sign = (shiftBits & 0x80);
+
+  /* If the shift value is positive then do right shift else left shift */
+  if(sign == 0u)
+  {
+    /* Initialize blkCnt with number of samples */
+    blkCnt = blockSize;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A << shiftBits */
+      /* Shift the input and then store the result in the destination buffer. */
+      *pDst++ = (q7_t) __SSAT(((q15_t) * pSrc++ << shiftBits), 8);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+  else
+  {
+    /* Initialize blkCnt with number of samples */
+    blkCnt = blockSize;
+
+    while(blkCnt > 0u)
+    {
+      /* C = A >> shiftBits */
+      /* Shift the input and then store the result in the destination buffer. */
+      *pDst++ = (*pSrc++ >> -shiftBits);
+
+      /* Decrement the loop counter */
+      blkCnt--;
+    }
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+}
+
+/**        
+ * @} end of shift group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sub_f32.c    
+*    
+* Description:	Floating-point vector subtraction.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupMath        
+ */
+
+/**        
+ * @defgroup BasicSub Vector Subtraction        
+ *        
+ * Element-by-element subtraction of two vectors.        
+ *        
+ * <pre>        
+ *     pDst[n] = pSrcA[n] - pSrcB[n],   0 <= n < blockSize.        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q7, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup BasicSub        
+ * @{        
+ */
+
+
+/**        
+ * @brief Floating-point vector subtraction.        
+ * @param[in]       *pSrcA points to the first input vector        
+ * @param[in]       *pSrcB points to the second input vector        
+ * @param[out]      *pDst points to the output vector        
+ * @param[in]       blockSize number of samples in each vector        
+ * @return none.        
+ */
+
+void arm_sub_f32(
+  float32_t * pSrcA,
+  float32_t * pSrcB,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t inA1, inA2, inA3, inA4;              /* temporary variables */
+  float32_t inB1, inB2, inB3, inB4;              /* temporary variables */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the results in the destination buffer. */
+    /* Read 4 input samples from sourceA and sourceB */
+    inA1 = *pSrcA;
+    inB1 = *pSrcB;
+    inA2 = *(pSrcA + 1);
+    inB2 = *(pSrcB + 1);
+    inA3 = *(pSrcA + 2);
+    inB3 = *(pSrcB + 2);
+    inA4 = *(pSrcA + 3);
+    inB4 = *(pSrcB + 3);
+
+    /* dst = srcA - srcB */
+    /* subtract and store the result */
+    *pDst = inA1 - inB1;
+    *(pDst + 1) = inA2 - inB2;
+    *(pDst + 2) = inA3 - inB3;
+    *(pDst + 3) = inA4 - inB4;
+
+
+    /* Update pointers to process next sampels */
+    pSrcA += 4u;
+    pSrcB += 4u;
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the results in the destination buffer. */
+    *pDst++ = (*pSrcA++) - (*pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of BasicSub group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c

@@ -0,0 +1,135 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sub_q15.c    
+*    
+* Description:	Q15 vector subtraction.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicSub    
+ * @{    
+ */
+
+/**    
+ * @brief Q15 vector subtraction.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.    
+ */
+
+void arm_sub_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inA1, inA2;
+  q31_t inB1, inB2;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the results in the destination buffer two samples at a time. */
+    inA1 = *__SIMD32(pSrcA)++;
+    inA2 = *__SIMD32(pSrcA)++;
+    inB1 = *__SIMD32(pSrcB)++;
+    inB2 = *__SIMD32(pSrcB)++;
+
+    *__SIMD32(pDst)++ = __QSUB16(inA1, inB1);
+    *__SIMD32(pDst)++ = __QSUB16(inA2, inB2);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the result in the destination buffer. */
+    *pDst++ = (q15_t) __QSUB16(*pSrcA++, *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the result in the destination buffer. */
+    *pDst++ = (q15_t) __SSAT(((q31_t) * pSrcA++ - *pSrcB++), 16);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+}
+
+/**    
+ * @} end of BasicSub group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c

@@ -0,0 +1,141 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sub_q31.c    
+*    
+* Description:	Q31 vector subtraction.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicSub    
+ * @{    
+ */
+
+/**    
+ * @brief Q31 vector subtraction.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated.    
+ */
+
+void arm_sub_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inA1, inA2, inA3, inA4;
+  q31_t inB1, inB2, inB3, inB4;
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the results in the destination buffer. */
+    inA1 = *pSrcA++;
+    inA2 = *pSrcA++;
+    inB1 = *pSrcB++;
+    inB2 = *pSrcB++;
+
+    inA3 = *pSrcA++;
+    inA4 = *pSrcA++;
+    inB3 = *pSrcB++;
+    inB4 = *pSrcB++;
+
+    *pDst++ = __QSUB(inA1, inB1);
+    *pDst++ = __QSUB(inA2, inB2);
+    *pDst++ = __QSUB(inA3, inB3);
+    *pDst++ = __QSUB(inA4, inB4);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the result in the destination buffer. */
+    *pDst++ = __QSUB(*pSrcA++, *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the result in the destination buffer. */
+    *pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrcA++ - *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of BasicSub group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c

@@ -0,0 +1,126 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sub_q7.c    
+*    
+* Description:	Q7 vector subtraction.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+*    
+* Version 0.0.7  2010/06/10     
+*    Misra-C changes done    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupMath    
+ */
+
+/**    
+ * @addtogroup BasicSub    
+ * @{    
+ */
+
+/**    
+ * @brief Q7 vector subtraction.    
+ * @param[in]       *pSrcA points to the first input vector    
+ * @param[in]       *pSrcB points to the second input vector    
+ * @param[out]      *pDst points to the output vector    
+ * @param[in]       blockSize number of samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.    
+ */
+
+void arm_sub_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  q7_t * pDst,
+  uint32_t blockSize)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+/* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  /*loop Unrolling */
+  blkCnt = blockSize >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the results in the destination buffer 4 samples at a time. */
+    *__SIMD32(pDst)++ = __QSUB8(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = blockSize % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the result in the destination buffer. */
+    *pDst++ = __SSAT(*pSrcA++ - *pSrcB++, 8);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* Initialize blkCnt with number of samples */
+  blkCnt = blockSize;
+
+  while(blkCnt > 0u)
+  {
+    /* C = A - B */
+    /* Subtract and then store the result in the destination buffer. */
+    *pDst++ = (q7_t) __SSAT((q15_t) * pSrcA++ - *pSrcB++, 8);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+
+}
+
+/**    
+ * @} end of BasicSub group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c

@@ -0,0 +1,174 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_conj_f32.c    
+*    
+* Description:	Floating-point complex conjugate.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupCmplxMath        
+ */
+
+/**        
+ * @defgroup cmplx_conj Complex Conjugate        
+ *        
+ * Conjugates the elements of a complex data vector.        
+ *       
+ * The <code>pSrc</code> points to the source data and        
+ * <code>pDst</code> points to the where the result should be written.        
+ * <code>numSamples</code> specifies the number of complex samples        
+ * and the data in each array is stored in an interleaved fashion        
+ * (real, imag, real, imag, ...).        
+ * Each array has a total of <code>2*numSamples</code> values.        
+ * The underlying algorithm is used:        
+ *        
+ * <pre>        
+ * for(n=0; n<numSamples; n++) {        
+ *     pDst[(2*n)+0)] = pSrc[(2*n)+0];     // real part        
+ *     pDst[(2*n)+1)] = -pSrc[(2*n)+1];    // imag part        
+ * }        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup cmplx_conj        
+ * @{        
+ */
+
+/**        
+ * @brief  Floating-point complex conjugate.        
+ * @param  *pSrc points to the input vector        
+ * @param  *pDst points to the output vector        
+ * @param  numSamples number of complex samples in each vector        
+ * @return none.        
+ */
+
+void arm_cmplx_conj_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t numSamples)
+{
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t inR1, inR2, inR3, inR4;
+  float32_t inI1, inI2, inI3, inI4;
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C[0]+jC[1] = A[0]+ j (-1) A[1] */
+    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
+    /* read real input samples */
+    inR1 = pSrc[0];
+    /* store real samples to destination */
+    pDst[0] = inR1;
+    inR2 = pSrc[2];
+    pDst[2] = inR2;
+    inR3 = pSrc[4];
+    pDst[4] = inR3;
+    inR4 = pSrc[6];
+    pDst[6] = inR4;
+
+    /* read imaginary input samples */
+    inI1 = pSrc[1];
+    inI2 = pSrc[3];
+
+    /* conjugate input */
+    inI1 = -inI1;
+
+    /* read imaginary input samples */
+    inI3 = pSrc[5];
+
+    /* conjugate input */
+    inI2 = -inI2;
+
+    /* read imaginary input samples */
+    inI4 = pSrc[7];
+
+    /* conjugate input */
+    inI3 = -inI3;
+
+    /* store imaginary samples to destination */
+    pDst[1] = inI1;
+    pDst[3] = inI2;
+
+    /* conjugate input */
+    inI4 = -inI4;
+
+    /* store imaginary samples to destination */
+    pDst[5] = inI3;
+
+    /* increment source pointer by 8 to process next sampels */
+    pSrc += 8u;
+
+    /* store imaginary sample to destination */
+    pDst[7] = inI4;
+
+    /* increment destination pointer by 8 to store next samples */
+    pDst += 8u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+  blkCnt = numSamples;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* realOut + j (imagOut) = realIn + j (-1) imagIn */
+    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
+    *pDst++ = *pSrc++;
+    *pDst++ = -*pSrc++;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of cmplx_conj group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c

@@ -0,0 +1,153 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_conj_q15.c    
+*    
+* Description:	Q15 complex conjugate.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @addtogroup cmplx_conj    
+ * @{    
+ */
+
+/**    
+ * @brief  Q15 complex conjugate.    
+ * @param  *pSrc points to the input vector    
+ * @param  *pDst points to the output vector    
+ * @param  numSamples number of complex samples in each vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function uses saturating arithmetic.    
+ * The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.    
+ */
+
+void arm_cmplx_conj_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t numSamples)
+{
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+  q31_t in1, in2, in3, in4;
+  q31_t zero = 0;
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C[0]+jC[1] = A[0]+ j (-1) A[1] */
+    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
+    in1 = *__SIMD32(pSrc)++;
+    in2 = *__SIMD32(pSrc)++;
+    in3 = *__SIMD32(pSrc)++;
+    in4 = *__SIMD32(pSrc)++;
+
+#ifndef ARM_MATH_BIG_ENDIAN
+
+    in1 = __QASX(zero, in1);
+    in2 = __QASX(zero, in2);
+    in3 = __QASX(zero, in3);
+    in4 = __QASX(zero, in4);
+
+#else
+
+    in1 = __QSAX(zero, in1);
+    in2 = __QSAX(zero, in2);
+    in3 = __QSAX(zero, in3);
+    in4 = __QSAX(zero, in4);
+
+#endif //       #ifndef ARM_MATH_BIG_ENDIAN
+
+    in1 = ((uint32_t) in1 >> 16) | ((uint32_t) in1 << 16);
+    in2 = ((uint32_t) in2 >> 16) | ((uint32_t) in2 << 16);
+    in3 = ((uint32_t) in3 >> 16) | ((uint32_t) in3 << 16);
+    in4 = ((uint32_t) in4 >> 16) | ((uint32_t) in4 << 16);
+
+    *__SIMD32(pDst)++ = in1;
+    *__SIMD32(pDst)++ = in2;
+    *__SIMD32(pDst)++ = in3;
+    *__SIMD32(pDst)++ = in4;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C[0]+jC[1] = A[0]+ j (-1) A[1] */
+    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
+    *pDst++ = *pSrc++;
+    *pDst++ = __SSAT(-*pSrc++, 16);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  q15_t in;
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* realOut + j (imagOut) = realIn+ j (-1) imagIn */
+    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
+    *pDst++ = *pSrc++;
+    in = *pSrc++;
+    *pDst++ = (in == (q15_t) 0x8000) ? 0x7fff : -in;
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of cmplx_conj group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c

@@ -0,0 +1,172 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_conj_q31.c    
+*    
+* Description:	Q31 complex conjugate.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupCmplxMath        
+ */
+
+/**        
+ * @addtogroup cmplx_conj        
+ * @{        
+ */
+
+/**        
+ * @brief  Q31 complex conjugate.        
+ * @param  *pSrc points to the input vector        
+ * @param  *pDst points to the output vector        
+ * @param  numSamples number of complex samples in each vector        
+ * @return none.        
+ *        
+ * <b>Scaling and Overflow Behavior:</b>        
+ * \par        
+ * The function uses saturating arithmetic.        
+ * The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.        
+ */
+
+void arm_cmplx_conj_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t numSamples)
+{
+  uint32_t blkCnt;                               /* loop counter */
+  q31_t in;                                      /* Input value */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t inR1, inR2, inR3, inR4;                  /* Temporary real variables */
+  q31_t inI1, inI2, inI3, inI4;                  /* Temporary imaginary variables */
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C[0]+jC[1] = A[0]+ j (-1) A[1] */
+    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
+    /* Saturated to 0x7fffffff if the input is -1(0x80000000) */
+    /* read real input sample */
+    inR1 = pSrc[0];
+    /* store real input sample */
+    pDst[0] = inR1;
+
+    /* read imaginary input sample */
+    inI1 = pSrc[1];
+
+    /* read real input sample */
+    inR2 = pSrc[2];
+    /* store real input sample */
+    pDst[2] = inR2;
+
+    /* read imaginary input sample */
+    inI2 = pSrc[3];
+
+    /* negate imaginary input sample */
+    inI1 = __QSUB(0, inI1);
+
+    /* read real input sample */
+    inR3 = pSrc[4];
+    /* store real input sample */
+    pDst[4] = inR3;
+
+    /* read imaginary input sample */
+    inI3 = pSrc[5];
+
+    /* negate imaginary input sample */
+    inI2 = __QSUB(0, inI2);
+
+    /* read real input sample */
+    inR4 = pSrc[6];
+    /* store real input sample */
+    pDst[6] = inR4;
+
+    /* negate imaginary input sample */
+    inI3 = __QSUB(0, inI3);
+
+    /* store imaginary input sample */
+    inI4 = pSrc[7];
+
+    /* store imaginary input samples */
+    pDst[1] = inI1;
+
+    /* negate imaginary input sample */
+    inI4 = __QSUB(0, inI4);
+
+    /* store imaginary input samples */
+    pDst[3] = inI2;
+
+    /* increment source pointer by 8 to proecess next samples */
+    pSrc += 8u;
+
+    /* store imaginary input samples */
+    pDst[5] = inI3;
+    pDst[7] = inI4;
+
+    /* increment destination pointer by 8 to process next samples */
+    pDst += 8u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+  blkCnt = numSamples;
+
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C[0]+jC[1] = A[0]+ j (-1) A[1] */
+    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
+    /* Saturated to 0x7fffffff if the input is -1(0x80000000) */
+    *pDst++ = *pSrc++;
+    in = *pSrc++;
+    *pDst++ = (in == 0x80000000) ? 0x7fffffff : -in;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of cmplx_conj group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c

@@ -0,0 +1,160 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_dot_prod_f32.c    
+*    
+* Description:	Floating-point complex dot product    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @defgroup cmplx_dot_prod Complex Dot Product    
+ *    
+ * Computes the dot product of two complex vectors.    
+ * The vectors are multiplied element-by-element and then summed.    
+ *   
+ * The <code>pSrcA</code> points to the first complex input vector and    
+ * <code>pSrcB</code> points to the second complex input vector.    
+ * <code>numSamples</code> specifies the number of complex samples    
+ * and the data in each array is stored in an interleaved fashion    
+ * (real, imag, real, imag, ...).    
+ * Each array has a total of <code>2*numSamples</code> values.    
+ *    
+ * The underlying algorithm is used:    
+ * <pre>    
+ * realResult=0;    
+ * imagResult=0;    
+ * for(n=0; n<numSamples; n++) {    
+ *     realResult += pSrcA[(2*n)+0]*pSrcB[(2*n)+0] - pSrcA[(2*n)+1]*pSrcB[(2*n)+1];    
+ *     imagResult += pSrcA[(2*n)+0]*pSrcB[(2*n)+1] + pSrcA[(2*n)+1]*pSrcB[(2*n)+0];    
+ * }    
+ * </pre>    
+ *    
+ * There are separate functions for floating-point, Q15, and Q31 data types.    
+ */
+
+/**    
+ * @addtogroup cmplx_dot_prod    
+ * @{    
+ */
+
+/**    
+ * @brief  Floating-point complex dot product    
+ * @param  *pSrcA points to the first input vector    
+ * @param  *pSrcB points to the second input vector    
+ * @param  numSamples number of complex samples in each vector    
+ * @param  *realResult real part of the result returned here    
+ * @param  *imagResult imaginary part of the result returned here    
+ * @return none.    
+ */
+
+void arm_cmplx_dot_prod_f32(
+  float32_t * pSrcA,
+  float32_t * pSrcB,
+  uint32_t numSamples,
+  float32_t * realResult,
+  float32_t * imagResult)
+{
+  float32_t real_sum = 0.0f, imag_sum = 0.0f;    /* Temporary result storage */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    real_sum += (*pSrcA++) * (*pSrcB++);
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    imag_sum += (*pSrcA++) * (*pSrcB++);
+
+    real_sum += (*pSrcA++) * (*pSrcB++);
+    imag_sum += (*pSrcA++) * (*pSrcB++);
+
+    real_sum += (*pSrcA++) * (*pSrcB++);
+    imag_sum += (*pSrcA++) * (*pSrcB++);
+
+    real_sum += (*pSrcA++) * (*pSrcB++);
+    imag_sum += (*pSrcA++) * (*pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    real_sum += (*pSrcA++) * (*pSrcB++);
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    imag_sum += (*pSrcA++) * (*pSrcB++);
+
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    real_sum += (*pSrcA++) * (*pSrcB++);
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    imag_sum += (*pSrcA++) * (*pSrcB++);
+
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  /* Store the real and imaginary results in the destination buffers */
+  *realResult = real_sum;
+  *imagResult = imag_sum;
+}
+
+/**    
+ * @} end of cmplx_dot_prod group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c

@@ -0,0 +1,144 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_dot_prod_q15.c    
+*    
+* Description:	Processing function for the Q15 Complex Dot product    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @addtogroup cmplx_dot_prod    
+ * @{    
+ */
+
+/**    
+ * @brief  Q15 complex dot product    
+ * @param  *pSrcA points to the first input vector    
+ * @param  *pSrcB points to the second input vector    
+ * @param  numSamples number of complex samples in each vector    
+ * @param  *realResult real part of the result returned here    
+ * @param  *imagResult imaginary part of the result returned here    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function is implemented using an internal 64-bit accumulator.    
+ * The intermediate 1.15 by 1.15 multiplications are performed with full precision and yield a 2.30 result.    
+ * These are accumulated in a 64-bit accumulator with 34.30 precision.    
+ * As a final step, the accumulators are converted to 8.24 format.    
+ * The return results <code>realResult</code> and <code>imagResult</code> are in 8.24 format.    
+ */
+
+void arm_cmplx_dot_prod_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  uint32_t numSamples,
+  q31_t * realResult,
+  q31_t * imagResult)
+{
+  q63_t real_sum = 0, imag_sum = 0;              /* Temporary result storage */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+
+    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+
+    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+
+    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  /* Store the real and imaginary results in 8.24 format  */
+  /* Convert real data in 34.30 to 8.24 by 6 right shifts */
+  *realResult = (q31_t) (real_sum) >> 6;
+  /* Convert imaginary data in 34.30 to 8.24 by 6 right shifts */
+  *imagResult = (q31_t) (imag_sum) >> 6;
+}
+
+/**    
+ * @} end of cmplx_dot_prod group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_dot_prod_q31.c    
+*    
+* Description:	Q31 complex dot product    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @addtogroup cmplx_dot_prod    
+ * @{    
+ */
+
+/**    
+ * @brief  Q31 complex dot product    
+ * @param  *pSrcA points to the first input vector    
+ * @param  *pSrcB points to the second input vector    
+ * @param  numSamples number of complex samples in each vector    
+ * @param  *realResult real part of the result returned here    
+ * @param  *imagResult imaginary part of the result returned here    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function is implemented using an internal 64-bit accumulator.    
+ * The intermediate 1.31 by 1.31 multiplications are performed with 64-bit precision and then shifted to 16.48 format.    
+ * The internal real and imaginary accumulators are in 16.48 format and provide 15 guard bits.    
+ * Additions are nonsaturating and no overflow will occur as long as <code>numSamples</code> is less than 32768.    
+ * The return results <code>realResult</code> and <code>imagResult</code> are in 16.48 format.    
+ * Input down scaling is not required.    
+ */
+
+void arm_cmplx_dot_prod_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  uint32_t numSamples,
+  q63_t * realResult,
+  q63_t * imagResult)
+{
+  q63_t real_sum = 0, imag_sum = 0;              /* Temporary result storage */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    /* Convert real data in 2.62 to 16.48 by 14 right shifts */
+    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    /* Convert imag data in 2.62 to 16.48 by 14 right shifts */
+    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+
+    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+
+    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+
+    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples  is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
+    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
+    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* outReal = realA[0]* realB[0] + realA[2]* realB[2] + realA[4]* realB[4] + .....+ realA[numSamples-2]* realB[numSamples-2] */
+    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+    /* outImag = imagA[1]* imagB[1] + imagA[3]* imagB[3] + imagA[5]* imagB[5] + .....+ imagA[numSamples-1]* imagB[numSamples-1] */
+    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  /* Store the real and imaginary results in 16.48 format  */
+  *realResult = real_sum;
+  *imagResult = imag_sum;
+}
+
+/**    
+ * @} end of cmplx_dot_prod group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c

@@ -0,0 +1,157 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_mag_f32.c    
+*    
+* Description:	Floating-point complex magnitude.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @defgroup cmplx_mag Complex Magnitude    
+ *    
+ * Computes the magnitude of the elements of a complex data vector.    
+ *   
+ * The <code>pSrc</code> points to the source data and    
+ * <code>pDst</code> points to the where the result should be written.    
+ * <code>numSamples</code> specifies the number of complex samples    
+ * in the input array and the data is stored in an interleaved fashion    
+ * (real, imag, real, imag, ...).    
+ * The input array has a total of <code>2*numSamples</code> values;    
+ * the output array has a total of <code>numSamples</code> values.    
+ * The underlying algorithm is used:    
+ *    
+ * <pre>    
+ * for(n=0; n<numSamples; n++) {    
+ *     pDst[n] = sqrt(pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2);    
+ * }    
+ * </pre>    
+ *    
+ * There are separate functions for floating-point, Q15, and Q31 data types.    
+ */
+
+/**    
+ * @addtogroup cmplx_mag    
+ * @{    
+ */
+/**    
+ * @brief Floating-point complex magnitude.    
+ * @param[in]       *pSrc points to complex input buffer    
+ * @param[out]      *pDst points to real output buffer    
+ * @param[in]       numSamples number of complex samples in the input vector    
+ * @return none.    
+ *    
+ */
+
+
+void arm_cmplx_mag_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t numSamples)
+{
+  float32_t realIn, imagIn;                      /* Temporary variables to hold input values */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+
+    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
+    realIn = *pSrc++;
+    imagIn = *pSrc++;
+    /* store the result in the destination buffer. */
+    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
+
+    realIn = *pSrc++;
+    imagIn = *pSrc++;
+    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
+
+    realIn = *pSrc++;
+    imagIn = *pSrc++;
+    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
+
+    realIn = *pSrc++;
+    imagIn = *pSrc++;
+    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
+
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
+    realIn = *pSrc++;
+    imagIn = *pSrc++;
+    /* store the result in the destination buffer. */
+    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* out = sqrt((real * real) + (imag * imag)) */
+    realIn = *pSrc++;
+    imagIn = *pSrc++;
+    /* store the result in the destination buffer. */
+    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of cmplx_mag group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_mag_q15.c    
+*    
+* Description:	Q15 complex magnitude.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @addtogroup cmplx_mag    
+ * @{    
+ */
+
+
+/**    
+ * @brief  Q15 complex magnitude    
+ * @param  *pSrc points to the complex input vector    
+ * @param  *pDst points to the real output vector    
+ * @param  numSamples number of complex samples in the input vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function implements 1.15 by 1.15 multiplications and finally output is converted into 2.14 format.    
+ */
+
+void arm_cmplx_mag_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t numSamples)
+{
+  q31_t acc0, acc1;                              /* Accumulators */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+  q31_t in1, in2, in3, in4;
+  q31_t acc2, acc3;
+
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+
+    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
+    in1 = *__SIMD32(pSrc)++;
+    in2 = *__SIMD32(pSrc)++;
+    in3 = *__SIMD32(pSrc)++;
+    in4 = *__SIMD32(pSrc)++;
+
+    acc0 = __SMUAD(in1, in1);
+    acc1 = __SMUAD(in2, in2);
+    acc2 = __SMUAD(in3, in3);
+    acc3 = __SMUAD(in4, in4);
+
+    /* store the result in 2.14 format in the destination buffer. */
+    arm_sqrt_q15((q15_t) ((acc0) >> 17), pDst++);
+    arm_sqrt_q15((q15_t) ((acc1) >> 17), pDst++);
+    arm_sqrt_q15((q15_t) ((acc2) >> 17), pDst++);
+    arm_sqrt_q15((q15_t) ((acc3) >> 17), pDst++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
+    in1 = *__SIMD32(pSrc)++;
+    acc0 = __SMUAD(in1, in1);
+
+    /* store the result in 2.14 format in the destination buffer. */
+    arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+  q15_t real, imag;                              /* Temporary variables to hold input values */
+
+  while(numSamples > 0u)
+  {
+    /* out = sqrt(real * real + imag * imag) */
+    real = *pSrc++;
+    imag = *pSrc++;
+
+    acc0 = (real * real);
+    acc1 = (imag * imag);
+
+    /* store the result in 2.14 format in the destination buffer. */
+    arm_sqrt_q15((q15_t) (((q63_t) acc0 + acc1) >> 17), pDst++);
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of cmplx_mag group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c

@@ -0,0 +1,177 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_mag_q31.c    
+*    
+* Description:	Q31 complex magnitude    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupCmplxMath        
+ */
+
+/**        
+ * @addtogroup cmplx_mag        
+ * @{        
+ */
+
+/**        
+ * @brief  Q31 complex magnitude        
+ * @param  *pSrc points to the complex input vector        
+ * @param  *pDst points to the real output vector        
+ * @param  numSamples number of complex samples in the input vector        
+ * @return none.        
+ *        
+ * <b>Scaling and Overflow Behavior:</b>        
+ * \par        
+ * The function implements 1.31 by 1.31 multiplications and finally output is converted into 2.30 format.        
+ * Input down scaling is not required.        
+ */
+
+void arm_cmplx_mag_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t numSamples)
+{
+  q31_t real, imag;                              /* Temporary variables to hold input values */
+  q31_t acc0, acc1;                              /* Accumulators */
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  q31_t real1, real2, imag1, imag2;              /* Temporary variables to hold input values */
+  q31_t out1, out2, out3, out4;                  /* Accumulators */
+  q63_t mul1, mul2, mul3, mul4;                  /* Temporary variables */
+
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* read complex input from source buffer */
+    real1 = pSrc[0];
+    imag1 = pSrc[1];
+    real2 = pSrc[2];
+    imag2 = pSrc[3];
+
+    /* calculate power of input values */
+    mul1 = (q63_t) real1 *real1;
+    mul2 = (q63_t) imag1 *imag1;
+    mul3 = (q63_t) real2 *real2;
+    mul4 = (q63_t) imag2 *imag2;
+
+    /* get the result to 3.29 format */
+    out1 = (q31_t) (mul1 >> 33);
+    out2 = (q31_t) (mul2 >> 33);
+    out3 = (q31_t) (mul3 >> 33);
+    out4 = (q31_t) (mul4 >> 33);
+
+    /* add real and imaginary accumulators */
+    out1 = out1 + out2;
+    out3 = out3 + out4;
+
+    /* read complex input from source buffer */
+    real1 = pSrc[4];
+    imag1 = pSrc[5];
+    real2 = pSrc[6];
+    imag2 = pSrc[7];
+
+    /* calculate square root */
+    arm_sqrt_q31(out1, &pDst[0]);
+
+    /* calculate power of input values */
+    mul1 = (q63_t) real1 *real1;
+
+    /* calculate square root */
+    arm_sqrt_q31(out3, &pDst[1]);
+
+    /* calculate power of input values */
+    mul2 = (q63_t) imag1 *imag1;
+    mul3 = (q63_t) real2 *real2;
+    mul4 = (q63_t) imag2 *imag2;
+
+    /* get the result to 3.29 format */
+    out1 = (q31_t) (mul1 >> 33);
+    out2 = (q31_t) (mul2 >> 33);
+    out3 = (q31_t) (mul3 >> 33);
+    out4 = (q31_t) (mul4 >> 33);
+
+    /* add real and imaginary accumulators */
+    out1 = out1 + out2;
+    out3 = out3 + out4;
+
+    /* calculate square root */
+    arm_sqrt_q31(out1, &pDst[2]);
+
+    /* increment destination by 8 to process next samples */
+    pSrc += 8u;
+
+    /* calculate square root */
+    arm_sqrt_q31(out3, &pDst[3]);
+
+    /* increment destination by 4 to process next samples */
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+  blkCnt = numSamples;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (q31_t) (((q63_t) real * real) >> 33);
+    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
+    /* store the result in 2.30 format in the destination buffer. */
+    arm_sqrt_q31(acc0 + acc1, pDst++);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of cmplx_mag group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c

@@ -0,0 +1,207 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_mag_squared_f32.c    
+*    
+* Description:	Floating-point complex magnitude squared.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupCmplxMath        
+ */
+
+/**        
+ * @defgroup cmplx_mag_squared Complex Magnitude Squared        
+ *        
+ * Computes the magnitude squared of the elements of a complex data vector.        
+ *       
+ * The <code>pSrc</code> points to the source data and        
+ * <code>pDst</code> points to the where the result should be written.        
+ * <code>numSamples</code> specifies the number of complex samples        
+ * in the input array and the data is stored in an interleaved fashion        
+ * (real, imag, real, imag, ...).        
+ * The input array has a total of <code>2*numSamples</code> values;        
+ * the output array has a total of <code>numSamples</code> values.        
+ *        
+ * The underlying algorithm is used:        
+ *        
+ * <pre>        
+ * for(n=0; n<numSamples; n++) {        
+ *     pDst[n] = pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2;        
+ * }        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup cmplx_mag_squared        
+ * @{        
+ */
+
+
+/**        
+ * @brief  Floating-point complex magnitude squared        
+ * @param[in]  *pSrc points to the complex input vector        
+ * @param[out]  *pDst points to the real output vector        
+ * @param[in]  numSamples number of complex samples in the input vector        
+ * @return none.        
+ */
+
+void arm_cmplx_mag_squared_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t numSamples)
+{
+  float32_t real, imag;                          /* Temporary variables to store real and imaginary values */
+  uint32_t blkCnt;                               /* loop counter */
+
+#ifndef ARM_MATH_CM0
+  float32_t real1, real2, real3, real4;          /* Temporary variables to hold real values */
+  float32_t imag1, imag2, imag3, imag4;          /* Temporary variables to hold imaginary values */
+  float32_t mul1, mul2, mul3, mul4;              /* Temporary variables */
+  float32_t mul5, mul6, mul7, mul8;              /* Temporary variables */
+  float32_t out1, out2, out3, out4;              /* Temporary variables to hold output values */
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
+    /* read real input sample from source buffer */
+    real1 = pSrc[0];
+    /* read imaginary input sample from source buffer */
+    imag1 = pSrc[1];
+
+    /* calculate power of real value */
+    mul1 = real1 * real1;
+
+    /* read real input sample from source buffer */
+    real2 = pSrc[2];
+
+    /* calculate power of imaginary value */
+    mul2 = imag1 * imag1;
+
+    /* read imaginary input sample from source buffer */
+    imag2 = pSrc[3];
+
+    /* calculate power of real value */
+    mul3 = real2 * real2;
+
+    /* read real input sample from source buffer */
+    real3 = pSrc[4];
+
+    /* calculate power of imaginary value */
+    mul4 = imag2 * imag2;
+
+    /* read imaginary input sample from source buffer */
+    imag3 = pSrc[5];
+
+    /* calculate power of real value */
+    mul5 = real3 * real3;
+    /* calculate power of imaginary value */
+    mul6 = imag3 * imag3;
+
+    /* read real input sample from source buffer */
+    real4 = pSrc[6];
+
+    /* accumulate real and imaginary powers */
+    out1 = mul1 + mul2;
+
+    /* read imaginary input sample from source buffer */
+    imag4 = pSrc[7];
+
+    /* accumulate real and imaginary powers */
+    out2 = mul3 + mul4;
+
+    /* calculate power of real value */
+    mul7 = real4 * real4;
+    /* calculate power of imaginary value */
+    mul8 = imag4 * imag4;
+
+    /* store output to destination */
+    pDst[0] = out1;
+
+    /* accumulate real and imaginary powers */
+    out3 = mul5 + mul6;
+
+    /* store output to destination */
+    pDst[1] = out2;
+
+    /* accumulate real and imaginary powers */
+    out4 = mul7 + mul8;
+
+    /* store output to destination */
+    pDst[2] = out3;
+
+    /* increment destination pointer by 8 to process next samples */
+    pSrc += 8u;
+
+    /* store output to destination */
+    pDst[3] = out4;
+
+    /* increment destination pointer by 4 to process next samples */
+    pDst += 4u;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  blkCnt = numSamples;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
+    real = *pSrc++;
+    imag = *pSrc++;
+
+    /* out = (real * real) + (imag * imag) */
+    /* store the result in the destination buffer. */
+    *pDst++ = (real * real) + (imag * imag);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of cmplx_mag_squared group        
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c

@@ -0,0 +1,140 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_mag_squared_q15.c    
+*    
+* Description:	Q15 complex magnitude squared.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @addtogroup cmplx_mag_squared    
+ * @{    
+ */
+
+/**    
+ * @brief  Q15 complex magnitude squared    
+ * @param  *pSrc points to the complex input vector    
+ * @param  *pDst points to the real output vector    
+ * @param  numSamples number of complex samples in the input vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function implements 1.15 by 1.15 multiplications and finally output is converted into 3.13 format.    
+ */
+
+void arm_cmplx_mag_squared_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t numSamples)
+{
+  q31_t acc0, acc1;                              /* Accumulators */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+  q31_t in1, in2, in3, in4;
+  q31_t acc2, acc3;
+
+  /*loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
+    in1 = *__SIMD32(pSrc)++;
+    in2 = *__SIMD32(pSrc)++;
+    in3 = *__SIMD32(pSrc)++;
+    in4 = *__SIMD32(pSrc)++;
+
+    acc0 = __SMUAD(in1, in1);
+    acc1 = __SMUAD(in2, in2);
+    acc2 = __SMUAD(in3, in3);
+    acc3 = __SMUAD(in4, in4);
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ = (q15_t) (acc0 >> 17);
+    *pDst++ = (q15_t) (acc1 >> 17);
+    *pDst++ = (q15_t) (acc2 >> 17);
+    *pDst++ = (q15_t) (acc3 >> 17);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
+    in1 = *__SIMD32(pSrc)++;
+    acc0 = __SMUAD(in1, in1);
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ = (q15_t) (acc0 >> 17);
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+  q15_t real, imag;                              /* Temporary variables to store real and imaginary values */
+
+  while(numSamples > 0u)
+  {
+    /* out = ((real * real) + (imag * imag)) */
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (real * real);
+    acc1 = (imag * imag);
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ = (q15_t) (((q63_t) acc0 + acc1) >> 17);
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of cmplx_mag_squared group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c

@@ -0,0 +1,153 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cmplx_mag_squared_q31.c    
+*    
+* Description:	Q31 complex magnitude squared.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* ---------------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupCmplxMath    
+ */
+
+/**    
+ * @addtogroup cmplx_mag_squared    
+ * @{    
+ */
+
+
+/**    
+ * @brief  Q31 complex magnitude squared    
+ * @param  *pSrc points to the complex input vector    
+ * @param  *pDst points to the real output vector    
+ * @param  numSamples number of complex samples in the input vector    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * The function implements 1.31 by 1.31 multiplications and finally output is converted into 3.29 format.    
+ * Input down scaling is not required.    
+ */
+
+void arm_cmplx_mag_squared_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t numSamples)
+{
+  q31_t real, imag;                              /* Temporary variables to store real and imaginary values */
+  q31_t acc0, acc1;                              /* Accumulators */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counter */
+
+  /* loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.    
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (q31_t) (((q63_t) real * real) >> 33);
+    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
+    /* store the result in 3.29 format in the destination buffer. */
+    *pDst++ = acc0 + acc1;
+
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (q31_t) (((q63_t) real * real) >> 33);
+    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
+    /* store the result in 3.29 format in the destination buffer. */
+    *pDst++ = acc0 + acc1;
+
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (q31_t) (((q63_t) real * real) >> 33);
+    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
+    /* store the result in 3.29 format in the destination buffer. */
+    *pDst++ = acc0 + acc1;
+
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (q31_t) (((q63_t) real * real) >> 33);
+    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
+    /* store the result in 3.29 format in the destination buffer. */
+    *pDst++ = acc0 + acc1;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+  while(blkCnt > 0u)
+  {
+    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (q31_t) (((q63_t) real * real) >> 33);
+    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
+    /* store the result in 3.29 format in the destination buffer. */
+    *pDst++ = acc0 + acc1;
+
+    /* Decrement the loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* out = ((real * real) + (imag * imag)) */
+    real = *pSrc++;
+    imag = *pSrc++;
+    acc0 = (q31_t) (((q63_t) real * real) >> 33);
+    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
+    /* store the result in 3.29 format in the destination buffer. */
+    *pDst++ = acc0 + acc1;
+
+    /* Decrement the loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of cmplx_mag_squared group    
+ */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c

@@ -0,0 +1,199 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_cmplx_mult_cmplx_f32.c    
+*    
+* Description:	Floating-point complex-by-complex multiplication    
+*    
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*  
+* Version 1.1.0 2012/02/15 
+*    Updated with more optimizations, bug fixes and minor API changes.  
+*   
+* Version 1.0.10 2011/7/15  
+*    Big Endian support added and Merged M0 and M3/M4 Source code.   
+*    
+* Version 1.0.3 2010/11/29   
+*    Re-organized the CMSIS folders and updated documentation.    
+*     
+* Version 1.0.2 2010/11/11    
+*    Documentation updated.     
+*    
+* Version 1.0.1 2010/10/05     
+*    Production release and review comments incorporated.    
+*    
+* Version 1.0.0 2010/09/20     
+*    Production release and review comments incorporated.    
+* -------------------------------------------------------------------- */
+#include "arm_math.h"
+
+/**        
+ * @ingroup groupCmplxMath        
+ */
+
+/**        
+ * @defgroup CmplxByCmplxMult Complex-by-Complex Multiplication        
+ *        
+ * Multiplies a complex vector by another complex vector and generates a complex result.        
+ * The data in the complex arrays is stored in an interleaved fashion        
+ * (real, imag, real, imag, ...).        
+ * The parameter <code>numSamples</code> represents the number of complex        
+ * samples processed.  The complex arrays have a total of <code>2*numSamples</code>        
+ * real values.        
+ *        
+ * The underlying algorithm is used:        
+ *        
+ * <pre>        
+ * for(n=0; n<numSamples; n++) {        
+ *     pDst[(2*n)+0] = pSrcA[(2*n)+0] * pSrcB[(2*n)+0] - pSrcA[(2*n)+1] * pSrcB[(2*n)+1];        
+ *     pDst[(2*n)+1] = pSrcA[(2*n)+0] * pSrcB[(2*n)+1] + pSrcA[(2*n)+1] * pSrcB[(2*n)+0];        
+ * }        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup CmplxByCmplxMult        
+ * @{        
+ */
+
+
+/**        
+ * @brief  Floating-point complex-by-complex multiplication        
+ * @param[in]  *pSrcA points to the first input vector        
+ * @param[in]  *pSrcB points to the second input vector        
+ * @param[out]  *pDst  points to the output vector        
+ * @param[in]  numSamples number of complex samples in each vector        
+ * @return none.        
+ */
+
+void arm_cmplx_mult_cmplx_f32(
+  float32_t * pSrcA,
+  float32_t * pSrcB,
+  float32_t * pDst,
+  uint32_t numSamples)
+{
+  float32_t a1, b1, c1, d1;                      /* Temporary variables to store real and imaginary values */
+  uint32_t blkCnt;                               /* loop counters */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  float32_t a2, b2, c2, d2;                      /* Temporary variables to store real and imaginary values */
+  float32_t acc1, acc2, acc3, acc4;
+
+
+  /* loop Unrolling */
+  blkCnt = numSamples >> 2u;
+
+  /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
+   ** a second loop below computes the remaining 1 to 3 samples. */
+  while(blkCnt > 0u)
+  {
+    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
+    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
+    a1 = *pSrcA;                /* A[2 * i] */
+    c1 = *pSrcB;                /* B[2 * i] */
+
+    b1 = *(pSrcA + 1);          /* A[2 * i + 1] */
+    acc1 = a1 * c1;             /* acc1 = A[2 * i] * B[2 * i] */
+
+    a2 = *(pSrcA + 2);          /* A[2 * i + 2] */
+    acc2 = (b1 * c1);           /* acc2 = A[2 * i + 1] * B[2 * i] */
+
+    d1 = *(pSrcB + 1);          /* B[2 * i + 1] */
+    c2 = *(pSrcB + 2);          /* B[2 * i + 2] */
+    acc1 -= b1 * d1;            /* acc1 =      A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1] */
+
+    d2 = *(pSrcB + 3);          /* B[2 * i + 3] */
+    acc3 = a2 * c2;             /* acc3 =       A[2 * i + 2] * B[2 * i + 2] */
+
+    b2 = *(pSrcA + 3);          /* A[2 * i + 3] */
+    acc2 += (a1 * d1);          /* acc2 =      A[2 * i + 1] * B[2 * i] + A[2 * i] * B[2 * i + 1] */
+
+    a1 = *(pSrcA + 4);          /* A[2 * i + 4] */
+    acc4 = (a2 * d2);           /* acc4 =   A[2 * i + 2] * B[2 * i + 3] */
+
+    c1 = *(pSrcB + 4);          /* B[2 * i + 4] */
+    acc3 -= (b2 * d2);          /* acc3 =       A[2 * i + 2] * B[2 * i + 2] - A[2 * i + 3] * B[2 * i + 3] */
+    *pDst = acc1;               /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1] */
+
+    b1 = *(pSrcA + 5);          /* A[2 * i + 5] */
+    acc4 += b2 * c2;            /* acc4 =   A[2 * i + 2] * B[2 * i + 3] + A[2 * i + 3] * B[2 * i + 2] */
+
+    *(pDst + 1) = acc2;         /* C[2 * i + 1] = A[2 * i + 1] * B[2 * i] + A[2 * i] * B[2 * i + 1]  */
+    acc1 = (a1 * c1);
+
+    d1 = *(pSrcB + 5);
+    acc2 = (b1 * c1);
+
+    *(pDst + 2) = acc3;
+    *(pDst + 3) = acc4;
+
+    a2 = *(pSrcA + 6);
+    acc1 -= (b1 * d1);
+
+    c2 = *(pSrcB + 6);
+    acc2 += (a1 * d1);
+
+    b2 = *(pSrcA + 7);
+    acc3 = (a2 * c2);
+
+    d2 = *(pSrcB + 7);
+    acc4 = (b2 * c2);
+
+    *(pDst + 4) = acc1;
+    pSrcA += 8u;
+
+    acc3 -= (b2 * d2);
+    acc4 += (a2 * d2);
+
+    *(pDst + 5) = acc2;
+    pSrcB += 8u;
+
+    *(pDst + 6) = acc3;
+    *(pDst + 7) = acc4;
+
+    pDst += 8u;
+
+    /* Decrement the numSamples loop counter */
+    blkCnt--;
+  }
+
+  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x4u;
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+  blkCnt = numSamples;
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  while(blkCnt > 0u)
+  {
+    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
+    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
+    a1 = *pSrcA++;
+    b1 = *pSrcA++;
+    c1 = *pSrcB++;
+    d1 = *pSrcB++;
+
+    /* store the result in the destination buffer. */
+    *pDst++ = (a1 * c1) - (b1 * d1);
+    *pDst++ = (a1 * d1) + (b1 * c1);
+
+    /* Decrement the numSamples loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of CmplxByCmplxMult group        
+ */