Merge branch 'master' of github.com:PX4/Firmware into fixedwing

Tools/fix_code_style_ubuntu.sh

@@ -0,0 +1,19 @@
+#!/bin/sh
+astyle \
+    --style=linux		\
+    --indent=force-tab=8	\
+    --indent-cases		\
+    --indent-preprocessor	\
+    --break-blocks=all		\
+    --pad-oper			\
+    --pad-header		\
+    --unpad-paren		\
+    --keep-one-line-blocks	\
+    --keep-one-line-statements	\
+    --align-pointer=name	\
+    --suffix=none		\
+    --lineend=linux		\
+    $*
+    #--ignore-exclude-errors-x	\
+    #--exclude=EASTL		\
+    #--align-reference=name	\

apps/controllib/Makefile

@@ -0,0 +1,53 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Control library
+#
+CSRCS		 = test_params.c
+
+CXXSRCS		 = block/Block.cpp \
+		   block/BlockParam.cpp \
+		   block/UOrbPublication.cpp \
+		   block/UOrbSubscription.cpp \
+		   blocks.cpp \
+		   fixedwing.cpp
+
+CXXHDRS		 = block/Block.hpp \
+		   block/BlockParam.hpp \
+		   block/UOrbPublication.hpp \
+		   block/UOrbSubscription.hpp \
+		   blocks.hpp \
+		   fixedwing.hpp
+
+include $(APPDIR)/mk/app.mk

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

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

apps/controllib/block/BlockParam.cpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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) :
+	_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 {
+			snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
+		}
+
+		parent->getParams().add(this);
+	}
+
+	_handle = param_find(fullname);
+
+	if (_handle == PARAM_INVALID)
+		printf("error finding param: %s\n", fullname);
+};
+
+} // namespace control

apps/controllib/block/BlockParam.hpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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:
+	BlockParamBase(Block *parent, const char *name);
+	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) :
+		BlockParamBase(block, name),
+		_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

apps/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;
+};

apps/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"

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

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

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

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

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

apps/controllib/fixedwing.cpp

@@ -0,0 +1,351 @@
+/****************************************************************************
+ *
+ *   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);
+}
+
+BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_psi2Phi(this, "PSI2PHI"),
+	_phi2P(this, "PHI2P"),
+	_phiLimit(this, "PHI_LIM")
+{
+}
+
+BlockHeadingHold::~BlockHeadingHold() {};
+
+float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
+{
+	float psiError = _wrap_pi(psiCmd - psi);
+	float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+	return _phi2P.update(phiCmd - phi);
+}
+
+BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Theta(this, "V2THE"),
+	_theta2Q(this, "THE2Q"),
+	_theLimit(this, "THE"),
+	_vLimit(this, "V")
+{
+}
+
+BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
+
+float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
+{
+	// negative sign because nose over to increase speed
+	float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Thr(this, "V2THR")
+{
+}
+
+BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
+
+float BlockVelocityHoldFrontside::update(float vCmd, float v)
+{
+	return _v2Thr.update(vCmd - v);
+}
+
+BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Thr(this, "H2THR"),
+	_throttle(0)
+{
+}
+
+BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
+
+void BlockAltitudeHoldBackside::update(float hCmd, float h)
+{
+	_throttle = _h2Thr.update(hCmd - h);
+}
+
+BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Theta(this, "H2THE"),
+	_theta2Q(this, "THE2Q")
+{
+}
+
+BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
+
+float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
+{
+	float thetaCmd = _h2Theta.update(hCmd - h);
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
+		const char *name,
+		BlockStabilization *stabilization) :
+	SuperBlock(parent, name),
+	_stabilization(stabilization),
+	_headingHold(this, ""),
+	_velocityHold(this, ""),
+	_altitudeHold(this, ""),
+	_trimAil(this, "TRIM_AIL"),
+	_trimElv(this, "TRIM_ELV"),
+	_trimRdr(this, "TRIM_RDR"),
+	_trimThr(this, "TRIM_THR")
+{
+}
+
+BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
+
+void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
+				    float h, float v,
+				    float phi, float theta, float psi,
+				    float p, float q, float r)
+{
+	_altitudeHold.update(hCmd, h);
+	_stabilization->update(
+		_headingHold.update(psiCmd, phi, psi, p),
+		_velocityHold.update(vCmd, v, theta, q),
+		rCmd,
+		p, q, 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
+	_backsideAutopilot(this, "", &_stabilization),
+	_guide(this, ""),
+	_vCmd(this, "V_CMD"),
+	_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;
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
+		_stabilization.update(
+			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.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;
+
+	} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// 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);
+
+		// commands
+		float rCmd = 0;
+
+		_backsideAutopilot.update(
+			_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
+			_pos.alt, v,
+			_att.roll, _att.pitch, _att.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.yawspeed
+		);
+		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
+		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
+		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
+		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
+
+	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+		_actuators.control[CH_AIL] = _manual.roll;
+		_actuators.control[CH_ELV] = _manual.pitch;
+		_actuators.control[CH_RDR] = _manual.yaw;
+		_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
+

apps/controllib/fixedwing.hpp

@@ -0,0 +1,438 @@
+/****************************************************************************
+ *
+ *   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(); }
+};
+
+/**
+ * Heading hold autopilot block.
+ * Aircraft Control and Simulation, Stevens and Lewis
+ * Heading hold, pg. 348
+ */
+class __EXPORT BlockHeadingHold : public SuperBlock
+{
+private:
+	BlockP _psi2Phi;
+	BlockP _phi2P;
+	BlockLimitSym _phiLimit;
+public:
+	BlockHeadingHold(SuperBlock *parent, const char *name);
+	virtual ~BlockHeadingHold();
+	/**
+	 * returns pCmd
+	 */
+	float update(float psiCmd, float phi, float psi, float p);
+};
+
+/**
+ * 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.
+ */
+
+/**
+ * Backside velocity hold autopilot block.
+ * v -> theta -> q -> elevator
+ */
+class __EXPORT BlockVelocityHoldBackside : public SuperBlock
+{
+private:
+	BlockPID _v2Theta;
+	BlockPID _theta2Q;
+	BlockLimit _theLimit;
+	BlockLimit _vLimit;
+public:
+	BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
+	virtual ~BlockVelocityHoldBackside();
+	/**
+	 * returns qCmd
+	 */
+	float update(float vCmd, float v, float theta, float q);
+};
+
+/**
+ * Frontside velocity hold autopilot block.
+ * v -> throttle
+ */
+class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
+{
+private:
+	BlockPID _v2Thr;
+public:
+	BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
+	virtual ~BlockVelocityHoldFrontside();
+	/**
+	 * returns throttle
+	 */
+	float update(float vCmd, float v);
+};
+
+/**
+ * Backside altitude hold autopilot block.
+ * h -> throttle
+ */
+class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
+{
+private:
+	BlockPID _h2Thr;
+	float _throttle;
+public:
+	BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
+	virtual ~BlockAltitudeHoldBackside();
+	void update(float hCmd, float h);
+	float getThrottle() { return _throttle; }
+};
+
+/**
+ * Frontside altitude hold autopilot block.
+ * h -> theta > q -> elevator
+ */
+class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
+{
+private:
+	BlockPID _h2Theta;
+	BlockPID _theta2Q;
+public:
+	BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
+	virtual ~BlockAltitudeHoldFrontside();
+	/**
+	 * return qCmd
+	 */
+	float update(float hCmd, float h, float theta, float q);
+};
+
+/**
+ * Backside autopilot
+ */
+class __EXPORT BlockBacksideAutopilot : public SuperBlock
+{
+private:
+	BlockStabilization *_stabilization;
+	BlockHeadingHold _headingHold;
+	BlockVelocityHoldBackside _velocityHold;
+	BlockAltitudeHoldBackside _altitudeHold;
+	BlockParam<float> _trimAil;
+	BlockParam<float> _trimElv;
+	BlockParam<float> _trimRdr;
+	BlockParam<float> _trimThr;
+public:
+	BlockBacksideAutopilot(SuperBlock *parent,
+			       const char *name,
+			       BlockStabilization *stabilization);
+	virtual ~BlockBacksideAutopilot();
+	void update(float hCmd, float vCmd, float rCmd, float psiCmd,
+		    float h, float v,
+		    float phi, float theta, float psi,
+		    float p, float q, float r);
+	float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
+	float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
+	float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
+	float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
+};
+
+/**
+ * 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:
+	BlockStabilization _stabilization;
+	BlockBacksideAutopilot _backsideAutopilot;
+	BlockWaypointGuidance _guide;
+	BlockParam<float> _vCmd;
+
+	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
+

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

apps/examples/control_demo/Makefile

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+APPNAME		 = control_demo
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk

apps/examples/control_demo/control_demo.cpp

@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   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 control_demo.cpp
+ * Demonstration of control library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <controllib/fixedwing.hpp>
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <math.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.
+ */
+extern "C" __EXPORT int control_demo_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int control_demo_thread_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: control_demo {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 control_demo_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("control_demo already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("control_demo",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 4096,
+					 control_demo_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "test")) {
+		test();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tcontrol_demo app is running\n");
+
+		} else {
+			printf("\tcontrol_demo app not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int control_demo_thread_main(int argc, char *argv[])
+{
+
+	printf("[control_Demo] starting\n");
+
+	using namespace control;
+
+	fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
+
+	thread_running = true;
+
+	while (!thread_should_exit) {
+		autopilot.update();
+	}
+
+	printf("[control_demo] exiting.\n");
+
+	thread_running = false;
+
+	return 0;
+}
+
+void test()
+{
+	printf("beginning control lib test\n");
+	control::basicBlocksTest();
+}

apps/examples/control_demo/params.c

@@ -0,0 +1,63 @@
+#include <systemlib/param/param.h>
+
+// currently tuned for easystar from arkhangar in HIL
+//https://github.com/arktools/arkhangar
+
+// 16 is max name length
+
+// gyro low pass filter
+PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
+
+// yaw washout
+PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
+
+// stabilization mode
+PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
+
+//  psi -> phi -> p
+PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f);      // heading 2 roll
+PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f);        // roll to roll rate
+PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f);  // roll limit
+
+// velocity -> theta
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
+
+
+// theta -> q
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
+
+//  h -> thr
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
+
+// crosstrack
+PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
+
+// speed command
+PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
+PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
+PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
+
+// trim
+PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);

apps/examples/kalman_demo/KalmanNav.cpp

@@ -0,0 +1,627 @@
+/****************************************************************************
+ *
+ *   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 KalmanNav.cpp
+ *
+ * kalman filter navigation code
+ */
+
+#include <poll.h>
+
+#include "KalmanNav.hpp"
+
+// constants
+static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
+static const float R = 6.371000e6f; // earth radius, m
+static const float RSq = 4.0589641e13f; // radius squared
+static const float g = 9.8f; // gravitational accel. m/s^2
+
+KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// ekf matrices
+	F(9, 9),
+	G(9, 6),
+	P(9, 9),
+	V(6, 6),
+	// attitude measurement ekf matrices
+	HAtt(6, 9),
+	RAtt(6, 6),
+	// gps measurement ekf matrices
+	HGps(6, 9),
+	RGps(6, 6),
+	// attitude representations
+	C_nb(),
+	q(),
+	// subscriptions
+	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
+	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
+	_att(&getPublications(), ORB_ID(vehicle_attitude)),
+	// timestamps
+	_pubTimeStamp(hrt_absolute_time()),
+	_fastTimeStamp(hrt_absolute_time()),
+	_slowTimeStamp(hrt_absolute_time()),
+	_attTimeStamp(hrt_absolute_time()),
+	_outTimeStamp(hrt_absolute_time()),
+	// frame count
+	_navFrames(0),
+	// state
+	fN(0), fE(0), fD(0),
+	phi(0), theta(0), psi(0),
+	vN(0), vE(0), vD(0),
+	lat(0), lon(0), alt(0),
+	// parameters for ground station
+	_vGyro(this, "V_GYRO"),
+	_vAccel(this, "V_ACCEL"),
+	_rMag(this, "R_MAG"),
+	_rGpsV(this, "R_GPS_V"),
+	_rGpsGeo(this, "R_GPS_GEO"),
+	_rGpsAlt(this, "R_GPS_ALT"),
+	_rAccel(this, "R_ACCEL")
+{
+	using namespace math;
+
+	// initial state covariance matrix
+	P = Matrix::identity(9) * 1.0f;
+
+	// wait for gps lock
+	while (1) {
+		updateSubscriptions();
+
+		if (_gps.fix_type > 2) break;
+
+		printf("[kalman_demo] waiting for gps lock\n");
+		usleep(1000000);
+	}
+
+	// initial state
+	phi = 0.0f;
+	theta = 0.0f;
+	psi = 0.0f;
+	vN = _gps.vel_n;
+	vE = _gps.vel_e;
+	vD = _gps.vel_d;
+	setLatDegE7(_gps.lat);
+	setLonDegE7(_gps.lon);
+	setAltE3(_gps.alt);
+
+	// initialize quaternions
+	q = Quaternion(EulerAngles(phi, theta, psi));
+
+	// initialize dcm
+	C_nb = Dcm(q);
+
+	// initialize F to identity
+	F = Matrix::identity(9);
+
+	// HGps is constant
+	HGps(0, 3) = 1.0f;
+	HGps(1, 4) = 1.0f;
+	HGps(2, 5) = 1.0f;
+	HGps(3, 6) = 1.0f;
+	HGps(4, 7) = 1.0f;
+	HGps(5, 8) = 1.0f;
+
+	// initialize all parameters
+	updateParams();
+}
+
+void KalmanNav::update()
+{
+	using namespace math;
+
+	struct pollfd fds[2];
+	fds[0].fd = _sensors.getHandle();
+	fds[0].events = POLLIN;
+	fds[1].fd = _param_update.getHandle();
+	fds[1].events = POLLIN;
+
+	// poll 20 milliseconds for new data
+	int ret = poll(fds, 2, 20);
+
+	// check return value
+	if (ret < 0) {
+		// XXX this is seriously bad - should be an emergency
+		return;
+
+	} else if (ret == 0) { // timeout
+		// run anyway
+	} else if (ret > 0) {
+		// update params when requested
+		if (fds[1].revents & POLLIN) {
+			printf("updating params\n");
+			updateParams();
+		}
+
+		// if no new sensor data, return
+		if (!(fds[0].revents & POLLIN)) return;
+	}
+
+	// get new timestamp
+	uint64_t newTimeStamp = hrt_absolute_time();
+
+	// check updated subscriptions
+	bool gpsUpdate = _gps.updated();
+
+	// get new information from subscriptions
+	// this clears update flag
+	updateSubscriptions();
+
+	// count fast frames
+	_navFrames += 1;
+
+	// fast prediciton step
+	// note, using sensors timestamp so we can account
+	// for packet lag
+	float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
+	_fastTimeStamp = _sensors.timestamp;
+	predictFast(dtFast);
+
+	// slow prediction step
+	float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
+
+	if (dtSlow > 1.0f / 200) { // 200 Hz
+		_slowTimeStamp = _sensors.timestamp;
+		predictSlow(dtSlow);
+	}
+
+	// gps correction step
+	if (gpsUpdate) {
+		correctGps();
+	}
+
+	// attitude correction step
+	if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
+		_attTimeStamp = _sensors.timestamp;
+		correctAtt();
+	}
+
+	// publication
+	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
+		_pubTimeStamp = newTimeStamp;
+		updatePublications();
+	}
+
+	// output
+	if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
+		_outTimeStamp = newTimeStamp;
+		printf("nav: %4d Hz\n", _navFrames);
+		_navFrames = 0;
+	}
+}
+
+void KalmanNav::updatePublications()
+{
+	using namespace math;
+
+	// position publication
+	_pos.timestamp = _pubTimeStamp;
+	_pos.time_gps_usec = _gps.timestamp;
+	_pos.valid = true;
+	_pos.lat = getLatDegE7();
+	_pos.lon = getLonDegE7();
+	_pos.alt = float(alt);
+	_pos.relative_alt = float(alt); // TODO, make relative
+	_pos.vx = vN;
+	_pos.vy = vE;
+	_pos.vz = vD;
+	_pos.hdg = psi;
+
+	// attitude publication
+	_att.timestamp = _pubTimeStamp;
+	_att.roll = phi;
+	_att.pitch = theta;
+	_att.yaw = psi;
+	_att.rollspeed = _sensors.gyro_rad_s[0];
+	_att.pitchspeed = _sensors.gyro_rad_s[1];
+	_att.yawspeed = _sensors.gyro_rad_s[2];
+	// TODO, add gyro offsets to filter
+	_att.rate_offsets[0] = 0.0f;
+	_att.rate_offsets[1] = 0.0f;
+	_att.rate_offsets[2] = 0.0f;
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+			_att.R[i][j] = C_nb(i, j);
+
+	for (int i = 0; i < 4; i++) _att.q[i] = q(i);
+
+	_att.R_valid = true;
+	_att.q_valid = true;
+	_att.counter = _navFrames;
+
+	// update publications
+	SuperBlock::updatePublications();
+}
+
+void KalmanNav::predictFast(float dt)
+{
+	using namespace math;
+	Vector3 w(_sensors.gyro_rad_s);
+
+	// attitude
+	q = q + q.derivative(w) * dt;
+
+	// renormalize quaternion if needed
+	if (fabsf(q.norm() - 1.0f) > 1e-4f) {
+		q = q.unit();
+	}
+
+	// C_nb update
+	C_nb = Dcm(q);
+
+	// euler update
+	EulerAngles euler(C_nb);
+	phi = euler.getPhi();
+	theta = euler.getTheta();
+	psi = euler.getPsi();
+
+	// specific acceleration in nav frame
+	Vector3 accelB(_sensors.accelerometer_m_s2);
+	Vector3 accelN = C_nb * accelB;
+	fN = accelN(0);
+	fE = accelN(1);
+	fD = accelN(2);
+
+	// trig
+	float sinL = sinf(lat);
+	float cosL = cosf(lat);
+
+	// position update
+	// neglects angular deflections in local gravity
+	// see Titerton pg. 70
+	float LDot = vN / (R + float(alt));
+	float lDot = vE / (cosL * (R + float(alt)));
+	float vNDot = fN - vE * (2 * omega +
+				 lDot) * sinL +
+		      vD * LDot;
+	float vDDot = fD - vE * (2 * omega + lDot) * cosL -
+		      vN * LDot + g;
+	float vEDot = fE + vN * (2 * omega + lDot) * sinL +
+		      vDDot * (2 * omega * cosL);
+
+	// rectangular integration
+	vN += vNDot * dt;
+	vE += vEDot * dt;
+	vD += vDDot * dt;
+	lat += double(LDot * dt);
+	lon += double(lDot * dt);
+	alt += double(-vD * dt);
+}
+
+void KalmanNav::predictSlow(float dt)
+{
+	using namespace math;
+
+	// trig
+	float sinL = sinf(lat);
+	float cosL = cosf(lat);
+	float cosLSq = cosL * cosL;
+	float tanL = tanf(lat);
+
+	// F Matrix
+	// Titterton pg. 291
+	//
+	// difference from Jacobian
+	// multiplity by dt for all elements
+	// add 1.0 to diagonal elements
+
+	F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt;
+	F(0, 2) = (vN / R) * dt;
+	F(0, 4) = (1.0f / R) * dt;
+	F(0, 6) = (-omega * sinL) * dt;
+	F(0, 8) = (-vE / RSq) * dt;
+
+	F(1, 0) = (omega * sinL + vE * tanL / R) * dt;
+	F(1, 2) = (omega * cosL + vE / R) * dt;
+	F(1, 3) = (-1.0f / R) * dt;
+	F(1, 8) = (vN / RSq) * dt;
+
+	F(2, 0) = (-vN / R) * dt;
+	F(2, 1) = (-omega * cosL - vE / R) * dt;
+	F(2, 4) = (-tanL / R) * dt;
+	F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt;
+	F(2, 8) = (vE * tanL / RSq) * dt;
+
+	F(3, 1) = (-fD) * dt;
+	F(3, 2) = (fE) * dt;
+	F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal
+	F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt;
+	F(3, 5) = (vN / R) * dt;
+	F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt;
+	F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt;
+
+	F(4, 0) = (fD) * dt;
+	F(4, 2) = (-fN) * dt;
+	F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt;
+	F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal
+	F(4, 5) = (2 * omega * cosL + vE / R) * dt;
+	F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) +
+		   vN * vE / (R * cosLSq)) * dt;
+	F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt;
+
+	F(5, 0) = (-fE) * dt;
+	F(5, 1) = (fN) * dt;
+	F(5, 3) = (-2 * vN / R) * dt;
+	F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt;
+	F(5, 6) = (2 * omega * vE * sinL) * dt;
+	F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt;
+
+	F(6, 3) = (1 / R) * dt;
+	F(6, 8) = (-vN / RSq) * dt;
+
+	F(7, 4) = (1 / (R * cosL)) * dt;
+	F(7, 6) = (vE * tanL / (R * cosL)) * dt;
+	F(7, 8) = (-vE / (cosL * RSq)) * dt;
+
+	F(8, 5) = (-1) * dt;
+
+	// G Matrix
+	// Titterton pg. 291
+	G(0, 0) = -C_nb(0, 0) * dt;
+	G(0, 1) = -C_nb(0, 1) * dt;
+	G(0, 2) = -C_nb(0, 2) * dt;
+	G(1, 0) = -C_nb(1, 0) * dt;
+	G(1, 1) = -C_nb(1, 1) * dt;
+	G(1, 2) = -C_nb(1, 2) * dt;
+	G(2, 0) = -C_nb(2, 0) * dt;
+	G(2, 1) = -C_nb(2, 1) * dt;
+	G(2, 2) = -C_nb(2, 2) * dt;
+
+	G(3, 3) = C_nb(0, 0) * dt;
+	G(3, 4) = C_nb(0, 1) * dt;
+	G(3, 5) = C_nb(0, 2) * dt;
+	G(4, 3) = C_nb(1, 0) * dt;
+	G(4, 4) = C_nb(1, 1) * dt;
+	G(4, 5) = C_nb(1, 2) * dt;
+	G(5, 3) = C_nb(2, 0) * dt;
+	G(5, 4) = C_nb(2, 1) * dt;
+	G(5, 5) = C_nb(2, 2) * dt;
+
+	// predict equations for kalman filter
+	P = F * P * F.transpose() + G * V * G.transpose();
+}
+
+void KalmanNav::correctAtt()
+{
+	using namespace math;
+
+	// trig
+	float cosPhi = cosf(phi);
+	float cosTheta = cosf(theta);
+	float cosPsi = cosf(psi);
+	float sinPhi = sinf(phi);
+	float sinTheta = sinf(theta);
+	float sinPsi = sinf(psi);
+
+	// mag measurement
+	Vector3 zMag(_sensors.magnetometer_ga);
+	zMag = zMag.unit();
+
+	// mag predicted measurement
+	// choosing some typical magnetic field properties,
+	//  TODO dip/dec depend on lat/ lon/ time
+	static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level
+	static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
+	float bN = cosf(dip) * cosf(dec);
+	float bE = cosf(dip) * sinf(dec);
+	float bD = sinf(dip);
+	Vector3 bNav(bN, bE, bD);
+	Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
+
+	// accel measurement
+	Vector3 zAccel(_sensors.accelerometer_m_s2);
+	zAccel = zAccel.unit();
+
+	// accel predicted measurement
+	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -1)).unit();
+
+	// combined measurement
+	Vector zAtt(6);
+	Vector zAttHat(6);
+
+	for (int i = 0; i < 3; i++) {
+		zAtt(i) = zMag(i);
+		zAtt(i + 3) = zAccel(i);
+		zAttHat(i) = zMagHat(i);
+		zAttHat(i + 3) = zAccelHat(i);
+	}
+
+	// HMag , HAtt (0-2,:)
+	float tmp1 =
+		cosPsi * cosTheta * bN +
+		sinPsi * cosTheta * bE -
+		sinTheta * bD;
+	HAtt(0, 1) = -(
+			     cosPsi * sinTheta * bN +
+			     sinPsi * sinTheta * bE +
+			     cosTheta * bD
+		     );
+	HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
+	HAtt(1, 0) =
+		(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
+		(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
+		cosPhi * cosTheta * bD;
+	HAtt(1, 1) = sinPhi * tmp1;
+	HAtt(1, 2) = -(
+			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
+			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
+		     );
+	HAtt(2, 0) = -(
+			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
+			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
+			     (sinPhi * cosTheta) * bD
+		     );
+	HAtt(2, 1) = cosPhi * tmp1;
+	HAtt(2, 2) = -(
+			     (cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
+			     (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
+		     );
+
+	// HAccel , HAtt (3-5,:)
+	HAtt(3, 1) = cosTheta;
+	HAtt(4, 0) = -cosPhi * cosTheta;
+	HAtt(4, 1) = sinPhi * sinTheta;
+	HAtt(5, 0) = sinPhi * cosTheta;
+	HAtt(5, 1) = cosPhi * sinTheta;
+
+	// compute correction
+	Vector y = zAtt - zAttHat; // residual
+	Matrix S = HAtt * P * HAtt.transpose() + RAtt; // residual covariance
+	Matrix K = P * HAtt.transpose() * S.inverse();
+	Vector xCorrect = K * y;
+
+	// check correciton is sane
+	for (size_t i = 0; i < xCorrect.getRows(); i++) {
+		float val = xCorrect(i);
+
+		if (isnan(val) || isinf(val)) {
+			// abort correction and return
+			printf("[kalman_demo] numerical failure in att correction\n");
+			return;
+		}
+	}
+
+	// correct state
+	phi += xCorrect(PHI);
+	theta += xCorrect(THETA);
+	psi += xCorrect(PSI);
+
+	// update state covariance
+	P = P - K * HAtt * P;
+
+	// fault detection
+	float beta = y.dot(S.inverse() * y);
+	printf("attitude: beta = %8.4f\n", (double)beta);
+
+	if (beta > 10.0f) {
+		//printf("fault in attitude: beta = %8.4f\n", (double)beta);
+		//printf("y:\n"); y.print();
+	}
+
+	// update quaternions from euler
+	// angle correction
+	q = Quaternion(EulerAngles(phi, theta, psi));
+}
+
+void KalmanNav::correctGps()
+{
+	using namespace math;
+	Vector y(6);
+	y(0) = _gps.vel_n - vN;
+	y(1) = _gps.vel_e - vE;
+	y(2) = _gps.vel_d - vD;
+	y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
+	y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
+	y(5) = double(_gps.alt) / 1.0e3 - alt;
+
+	// compute correction
+	Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance
+	Matrix K = P * HGps.transpose() * S.inverse();
+	Vector xCorrect = K * y;
+
+	// check correction is sane
+	for (size_t i = 0; i < xCorrect.getRows(); i++) {
+		float val = xCorrect(i);
+
+		if (isnan(val) || isinf(val)) {
+			// abort correction and return
+			printf("[kalman_demo] numerical failure in gps correction\n");
+			// fallback to GPS
+			vN = _gps.vel_n;
+			vE = _gps.vel_e;
+			vD = _gps.vel_d;
+			setLatDegE7(_gps.lat);
+			setLonDegE7(_gps.lon);
+			setAltE3(_gps.alt);
+			return;
+		}
+	}
+
+	// correct state
+	vN += xCorrect(VN);
+	vE += xCorrect(VE);
+	vD += xCorrect(VD);
+	lat += double(xCorrect(LAT));
+	lon += double(xCorrect(LON));
+	alt += double(xCorrect(ALT));
+
+	// update state covariance
+	P = P - K * HGps * P;
+
+	// fault detetcion
+	float beta = y.dot(S.inverse() * y);
+	printf("gps: beta = %8.4f\n", (double)beta);
+
+	if (beta > 100.0f) {
+		//printf("fault in gps: beta = %8.4f\n", (double)beta);
+		//printf("y:\n"); y.print();
+	}
+}
+
+void KalmanNav::updateParams()
+{
+	using namespace math;
+	using namespace control;
+	SuperBlock::updateParams();
+
+	// gyro noise
+	V(0, 0) = _vGyro.get();   // gyro x, rad/s
+	V(1, 1) = _vGyro.get();   // gyro y
+	V(2, 2) = _vGyro.get();   // gyro z
+
+	// accel noise
+	V(3, 3) = _vAccel.get();   // accel x, m/s^2
+	V(4, 4) = _vAccel.get();   // accel y
+	V(5, 5) = _vAccel.get();   // accel z
+
+	// magnetometer noise
+	RAtt(0, 0) = _rMag.get(); // normalized direction
+	RAtt(1, 1) = _rMag.get();
+	RAtt(2, 2) = _rMag.get();
+
+	// accelerometer noise
+	RAtt(3, 3) = _rAccel.get(); // normalized direction
+	RAtt(4, 4) = _rAccel.get();
+	RAtt(5, 5) = _rAccel.get();
+
+	// gps noise
+	RGps(0, 0) = _rGpsV.get(); // vn, m/s
+	RGps(1, 1) = _rGpsV.get(); // ve
+	RGps(2, 2) = _rGpsV.get(); // vd
+	RGps(3, 3) = _rGpsGeo.get(); // L, rad
+	RGps(4, 4) = _rGpsGeo.get(); // l, rad
+	RGps(5, 5) = _rGpsAlt.get(); // h, m
+}

apps/examples/kalman_demo/KalmanNav.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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 KalmanNav.hpp
+ *
+ * kalman filter navigation code
+ */
+
+#pragma once
+
+//#define MATRIX_ASSERT
+//#define VECTOR_ASSERT
+
+#include <nuttx/config.h>
+
+#include <mathlib/mathlib.h>
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <controllib/block/UOrbSubscription.hpp>
+#include <controllib/block/UOrbPublication.hpp>
+
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+#include <unistd.h>
+
+class KalmanNav : public control::SuperBlock
+{
+public:
+	KalmanNav(SuperBlock *parent, const char *name);
+	virtual ~KalmanNav() {};
+	void update();
+	virtual void updatePublications();
+	void predictFast(float dt);
+	void predictSlow(float dt);
+	void correctAtt();
+	void correctGps();
+	virtual void updateParams();
+protected:
+	math::Matrix F;
+	math::Matrix G;
+	math::Matrix P;
+	math::Matrix V;
+	math::Matrix HAtt;
+	math::Matrix RAtt;
+	math::Matrix HGps;
+	math::Matrix RGps;
+	math::Dcm C_nb;
+	math::Quaternion q;
+	control::UOrbSubscription<sensor_combined_s> _sensors;
+	control::UOrbSubscription<vehicle_gps_position_s> _gps;
+	control::UOrbSubscription<parameter_update_s> _param_update;
+	control::UOrbPublication<vehicle_global_position_s> _pos;
+	control::UOrbPublication<vehicle_attitude_s> _att;
+	uint64_t _pubTimeStamp;
+	uint64_t _fastTimeStamp;
+	uint64_t _slowTimeStamp;
+	uint64_t _attTimeStamp;
+	uint64_t _outTimeStamp;
+	uint16_t _navFrames;
+	float fN, fE, fD;
+	// states
+	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
+	float phi, theta, psi;
+	float vN, vE, vD;
+	double lat, lon, alt;
+	control::BlockParam<float> _vGyro;
+	control::BlockParam<float> _vAccel;
+	control::BlockParam<float> _rMag;
+	control::BlockParam<float> _rGpsV;
+	control::BlockParam<float> _rGpsGeo;
+	control::BlockParam<float> _rGpsAlt;
+	control::BlockParam<float> _rAccel;
+	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
+	void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
+	int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
+	void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
+	int32_t getAltE3() { return int32_t(alt * 1.0e3); }
+	void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
+};

apps/examples/kalman_demo/Makefile

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+APPNAME		 = kalman_demo
+PRIORITY	 = SCHED_PRIORITY_MAX - 30
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk

apps/examples/kalman_demo/kalman_demo.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   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 kalman_demo.cpp
+ * Demonstration of control library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+#include "KalmanNav.hpp"
+
+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.
+ */
+extern "C" __EXPORT int kalman_demo_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int kalman_demo_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: kalman_demo {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 kalman_demo_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("kalman_demo already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("kalman_demo",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 4096,
+					 kalman_demo_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("\tkalman_demo app is running\n");
+
+		} else {
+			printf("\tkalman_demo app not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int kalman_demo_thread_main(int argc, char *argv[])
+{
+
+	printf("[kalman_demo] starting\n");
+
+	using namespace math;
+
+	thread_running = true;
+
+	KalmanNav nav(NULL, "KF");
+
+	while (!thread_should_exit) {
+		nav.update();
+	}
+
+	printf("[kalman_demo] exiting.\n");
+
+	thread_running = false;
+
+	return 0;
+}

apps/examples/kalman_demo/params.c

@@ -0,0 +1,10 @@
+#include <systemlib/param/param.h>
+
+/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);

apps/examples/math_demo/Makefile

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+APPNAME		 = math_demo
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 8192
+
+include $(APPDIR)/mk/app.mk

apps/examples/math_demo/math_demo.cpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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 math_demo.cpp
+ * 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();
+}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c

@@ -0,0 +1,185 @@
+/* ----------------------------------------------------------------------    
+* 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_q15.c    
+*    
+* Description:	Q15 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    
+ */
+
+/**    
+ * @addtogroup CmplxByCmplxMult    
+ * @{    
+ */
+
+/**    
+ * @brief  Q15 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.    
+ *    
+ * <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_mult_cmplx_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t numSamples)
+{
+  q15_t a, b, c, d;                              /* Temporary variables to store real and imaginary values */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counters */
+
+  /* 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].  */
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
+
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
+
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
+
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
+
+    /* 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 = numSamples % 0x4u;
+
+  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].  */
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 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].  */
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
+    /* store the result in 3.13 format in the destination buffer. */
+    *pDst++ =
+      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
+
+    /* Decrement the blockSize loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of CmplxByCmplxMult group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c

@@ -0,0 +1,318 @@
+/* ----------------------------------------------------------------------    
+* 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_q31.c    
+*    
+* Description:	Q31 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    
+ */
+
+/**    
+ * @addtogroup CmplxByCmplxMult    
+ * @{    
+ */
+
+
+/**    
+ * @brief  Q31 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.    
+ *    
+ * <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_mult_cmplx_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t numSamples)
+{
+  q31_t a, b, c, d;                              /* Temporary variables to store real and imaginary values */
+  uint32_t blkCnt;                               /* loop counters */
+  q31_t mul1, mul2, mul3, mul4;
+  q31_t out1, out2;
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  /* 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].  */
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    /* 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 = numSamples % 0x4u;
+
+  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].  */
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  /* loop Unrolling */
+  blkCnt = numSamples >> 1u;
+
+  /* First part of the processing with loop unrolling.  Compute 2 outputs at a time.     
+   ** a second loop below computes the remaining 1 sample. */
+  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].  */
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+  /* If the blockSize is not a multiple of 2, compute any remaining output samples here.     
+   ** No loop unrolling is used. */
+  blkCnt = numSamples % 0x2u;
+
+  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].  */
+    a = *pSrcA++;
+    b = *pSrcA++;
+    c = *pSrcB++;
+    d = *pSrcB++;
+
+    mul1 = (q31_t) (((q63_t) a * c) >> 32);
+    mul2 = (q31_t) (((q63_t) b * d) >> 32);
+    mul3 = (q31_t) (((q63_t) a * d) >> 32);
+    mul4 = (q31_t) (((q63_t) b * c) >> 32);
+
+    mul1 = (mul1 >> 1);
+    mul2 = (mul2 >> 1);
+    mul3 = (mul3 >> 1);
+    mul4 = (mul4 >> 1);
+
+    out1 = mul1 - mul2;
+    out2 = mul3 + mul4;
+
+    /* store the real result in 3.29 format in the destination buffer. */
+    *pDst++ = out1;
+    /* store the imag result in 3.29 format in the destination buffer. */
+    *pDst++ = out2;
+
+    /* Decrement the blockSize loop counter */
+    blkCnt--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of CmplxByCmplxMult group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c

@@ -0,0 +1,217 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_cmplx_mult_real_f32.c    
+*    
+* Description:	Floating-point complex by real 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 CmplxByRealMult Complex-by-Real Multiplication        
+ *        
+ * Multiplies a complex vector by a real 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 while the real array has a total of <code>numSamples</code>        
+ * real values.        
+ *        
+ * The underlying algorithm is used:        
+ *        
+ * <pre>        
+ * for(n=0; n<numSamples; n++) {        
+ *     pCmplxDst[(2*n)+0] = pSrcCmplx[(2*n)+0] * pSrcReal[n];        
+ *     pCmplxDst[(2*n)+1] = pSrcCmplx[(2*n)+1] * pSrcReal[n];        
+ * }        
+ * </pre>        
+ *        
+ * There are separate functions for floating-point, Q15, and Q31 data types.        
+ */
+
+/**        
+ * @addtogroup CmplxByRealMult        
+ * @{        
+ */
+
+
+/**        
+ * @brief  Floating-point complex-by-real multiplication        
+ * @param[in]  *pSrcCmplx points to the complex input vector        
+ * @param[in]  *pSrcReal points to the real input vector        
+ * @param[out]  *pCmplxDst points to the complex output vector        
+ * @param[in]  numSamples number of samples in each vector        
+ * @return none.        
+ */
+
+void arm_cmplx_mult_real_f32(
+  float32_t * pSrcCmplx,
+  float32_t * pSrcReal,
+  float32_t * pCmplxDst,
+  uint32_t numSamples)
+{
+  float32_t in;                                  /* Temporary variable to store input value */
+  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 variables to hold input data */
+  float32_t inA5, inA6, inA7, inA8;              /* Temporary variables to hold input data */
+  float32_t inB1, inB2, inB3, inB4;              /* Temporary variables to hold input data */
+  float32_t out1, out2, out3, out4;              /* Temporary variables to hold output data */
+  float32_t out5, out6, out7, out8;              /* Temporary variables to hold output data */
+
+  /* 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[i].            */
+    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
+    /* read input from complex input buffer */
+    inA1 = pSrcCmplx[0];
+    inA2 = pSrcCmplx[1];
+    /* read input from real input buffer */
+    inB1 = pSrcReal[0];
+
+    /* read input from complex input buffer */
+    inA3 = pSrcCmplx[2];
+
+    /* multiply complex buffer real input with real buffer input */
+    out1 = inA1 * inB1;
+
+    /* read input from complex input buffer */
+    inA4 = pSrcCmplx[3];
+
+    /* multiply complex buffer imaginary input with real buffer input */
+    out2 = inA2 * inB1;
+
+    /* read input from real input buffer */
+    inB2 = pSrcReal[1];
+    /* read input from complex input buffer */
+    inA5 = pSrcCmplx[4];
+
+    /* multiply complex buffer real input with real buffer input */
+    out3 = inA3 * inB2;
+
+    /* read input from complex input buffer */
+    inA6 = pSrcCmplx[5];
+    /* read input from real input buffer */
+    inB3 = pSrcReal[2];
+
+    /* multiply complex buffer imaginary input with real buffer input */
+    out4 = inA4 * inB2;
+
+    /* read input from complex input buffer */
+    inA7 = pSrcCmplx[6];
+
+    /* multiply complex buffer real input with real buffer input */
+    out5 = inA5 * inB3;
+
+    /* read input from complex input buffer */
+    inA8 = pSrcCmplx[7];
+
+    /* multiply complex buffer imaginary input with real buffer input */
+    out6 = inA6 * inB3;
+
+    /* read input from real input buffer */
+    inB4 = pSrcReal[3];
+
+    /* store result to destination bufer */
+    pCmplxDst[0] = out1;
+
+    /* multiply complex buffer real input with real buffer input */
+    out7 = inA7 * inB4;
+
+    /* store result to destination bufer */
+    pCmplxDst[1] = out2;
+
+    /* multiply complex buffer imaginary input with real buffer input */
+    out8 = inA8 * inB4;
+
+    /* store result to destination bufer */
+    pCmplxDst[2] = out3;
+    pCmplxDst[3] = out4;
+    pCmplxDst[4] = out5;
+
+    /* incremnet complex input buffer by 8 to process next samples */
+    pSrcCmplx += 8u;
+
+    /* store result to destination bufer */
+    pCmplxDst[5] = out6;
+
+    /* increment real input buffer by 4 to process next samples */
+    pSrcReal += 4u;
+
+    /* store result to destination bufer */
+    pCmplxDst[6] = out7;
+    pCmplxDst[7] = out8;
+
+    /* increment destination buffer by 8 to process next sampels */
+    pCmplxDst += 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[i].            */
+    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
+    in = *pSrcReal++;
+    /* store the result in the destination buffer. */
+    *pCmplxDst++ = (*pSrcCmplx++) * (in);
+    *pCmplxDst++ = (*pSrcCmplx++) * (in);
+
+    /* Decrement the numSamples loop counter */
+    blkCnt--;
+  }
+}
+
+/**        
+ * @} end of CmplxByRealMult group        
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.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_cmplx_mult_real_q15.c    
+*    
+* Description:	Q15 complex by real 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    
+ */
+
+/**    
+ * @addtogroup CmplxByRealMult    
+ * @{    
+ */
+
+
+/**    
+ * @brief  Q15 complex-by-real multiplication    
+ * @param[in]  *pSrcCmplx points to the complex input vector    
+ * @param[in]  *pSrcReal points to the real input vector    
+ * @param[out]  *pCmplxDst points to the complex output vector    
+ * @param[in]  numSamples 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_cmplx_mult_real_q15(
+  q15_t * pSrcCmplx,
+  q15_t * pSrcReal,
+  q15_t * pCmplxDst,
+  uint32_t numSamples)
+{
+  q15_t in;                                      /* Temporary variable to store input value */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counters */
+  q31_t inA1, inA2;                              /* Temporary variables to hold input data */
+  q31_t inB1;                                    /* Temporary variables to hold input data */
+  q15_t out1, out2, out3, out4;                  /* Temporary variables to hold output data */
+  q31_t mul1, mul2, mul3, mul4;                  /* Temporary variables to hold intermediate data */
+
+  /* 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[i].            */
+    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
+    /* read complex number both real and imaginary from complex input buffer */
+    inA1 = *__SIMD32(pSrcCmplx)++;
+    /* read two real values at a time from real input buffer */
+    inB1 = *__SIMD32(pSrcReal)++;
+    /* read complex number both real and imaginary from complex input buffer */
+    inA2 = *__SIMD32(pSrcCmplx)++;
+
+    /* multiply complex number with real numbers */
+#ifndef ARM_MATH_BIG_ENDIAN
+
+    mul1 = (q31_t) ((q15_t) (inA1) * (q15_t) (inB1));
+    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1));
+    mul3 = (q31_t) ((q15_t) (inA2) * (q15_t) (inB1 >> 16));
+    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB1 >> 16));
+
+#else
+
+    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
+    mul1 = (q31_t) ((q15_t) inA1 * (q15_t) (inB1 >> 16));
+    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) inB1);
+    mul3 = (q31_t) ((q15_t) inA2 * (q15_t) inB1);
+
+#endif //      #ifndef ARM_MATH_BIG_ENDIAN
+
+    /* saturate the result */
+    out1 = (q15_t) __SSAT(mul1 >> 15u, 16);
+    out2 = (q15_t) __SSAT(mul2 >> 15u, 16);
+    out3 = (q15_t) __SSAT(mul3 >> 15u, 16);
+    out4 = (q15_t) __SSAT(mul4 >> 15u, 16);
+
+    /* pack real and imaginary outputs and store them to destination */
+    *__SIMD32(pCmplxDst)++ = __PKHBT(out1, out2, 16);
+    *__SIMD32(pCmplxDst)++ = __PKHBT(out3, out4, 16);
+
+    inA1 = *__SIMD32(pSrcCmplx)++;
+    inB1 = *__SIMD32(pSrcReal)++;
+    inA2 = *__SIMD32(pSrcCmplx)++;
+
+#ifndef ARM_MATH_BIG_ENDIAN
+
+    mul1 = (q31_t) ((q15_t) (inA1) * (q15_t) (inB1));
+    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1));
+    mul3 = (q31_t) ((q15_t) (inA2) * (q15_t) (inB1 >> 16));
+    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB1 >> 16));
+
+#else
+
+    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
+    mul1 = (q31_t) ((q15_t) inA1 * (q15_t) (inB1 >> 16));
+    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) inB1);
+    mul3 = (q31_t) ((q15_t) inA2 * (q15_t) inB1);
+
+#endif //      #ifndef ARM_MATH_BIG_ENDIAN
+
+    out1 = (q15_t) __SSAT(mul1 >> 15u, 16);
+    out2 = (q15_t) __SSAT(mul2 >> 15u, 16);
+    out3 = (q15_t) __SSAT(mul3 >> 15u, 16);
+    out4 = (q15_t) __SSAT(mul4 >> 15u, 16);
+
+    *__SIMD32(pCmplxDst)++ = __PKHBT(out1, out2, 16);
+    *__SIMD32(pCmplxDst)++ = __PKHBT(out3, out4, 16);
+
+    /* 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;
+
+  while(blkCnt > 0u)
+  {
+    /* C[2 * i] = A[2 * i] * B[i].            */
+    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
+    in = *pSrcReal++;
+    /* store the result in the destination buffer. */
+    *pCmplxDst++ =
+      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
+    *pCmplxDst++ =
+      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
+
+    /* Decrement the numSamples loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* realOut = realA * realB.            */
+    /* imagOut = imagA * realB.                */
+    in = *pSrcReal++;
+    /* store the result in the destination buffer. */
+    *pCmplxDst++ =
+      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
+    *pCmplxDst++ =
+      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
+
+    /* Decrement the numSamples loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of CmplxByRealMult group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.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_cmplx_mult_real_q31.c    
+*    
+* Description:	Q31 complex by real 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    
+ */
+
+/**    
+ * @addtogroup CmplxByRealMult    
+ * @{    
+ */
+
+
+/**    
+ * @brief  Q31 complex-by-real multiplication    
+ * @param[in]  *pSrcCmplx points to the complex input vector    
+ * @param[in]  *pSrcReal points to the real input vector    
+ * @param[out]  *pCmplxDst points to the complex output vector    
+ * @param[in]  numSamples 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_cmplx_mult_real_q31(
+  q31_t * pSrcCmplx,
+  q31_t * pSrcReal,
+  q31_t * pCmplxDst,
+  uint32_t numSamples)
+{
+  q31_t inA1;                                    /* Temporary variable to store input value */
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+  uint32_t blkCnt;                               /* loop counters */
+  q31_t inA2, inA3, inA4;                        /* Temporary variables to hold input data */
+  q31_t inB1, inB2;                              /* Temporary variabels to hold input data */
+  q31_t out1, out2, out3, out4;                  /* Temporary variables to hold output data */
+
+  /* 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[i].            */
+    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
+    /* read real input from complex input buffer */
+    inA1 = *pSrcCmplx++;
+    inA2 = *pSrcCmplx++;
+    /* read input from real input bufer */
+    inB1 = *pSrcReal++;
+    inB2 = *pSrcReal++;
+    /* read imaginary input from complex input buffer */
+    inA3 = *pSrcCmplx++;
+    inA4 = *pSrcCmplx++;
+
+    /* multiply complex input with real input */
+    out1 = ((q63_t) inA1 * inB1) >> 32;
+    out2 = ((q63_t) inA2 * inB1) >> 32;
+    out3 = ((q63_t) inA3 * inB2) >> 32;
+    out4 = ((q63_t) inA4 * inB2) >> 32;
+
+    /* sature the result */
+    out1 = __SSAT(out1, 31);
+    out2 = __SSAT(out2, 31);
+    out3 = __SSAT(out3, 31);
+    out4 = __SSAT(out4, 31);
+
+    /* get result in 1.31 format */
+    out1 = out1 << 1;
+    out2 = out2 << 1;
+    out3 = out3 << 1;
+    out4 = out4 << 1;
+
+    /* store the result to destination buffer */
+    *pCmplxDst++ = out1;
+    *pCmplxDst++ = out2;
+    *pCmplxDst++ = out3;
+    *pCmplxDst++ = out4;
+
+    /* read real input from complex input buffer */
+    inA1 = *pSrcCmplx++;
+    inA2 = *pSrcCmplx++;
+    /* read input from real input bufer */
+    inB1 = *pSrcReal++;
+    inB2 = *pSrcReal++;
+    /* read imaginary input from complex input buffer */
+    inA3 = *pSrcCmplx++;
+    inA4 = *pSrcCmplx++;
+
+    /* multiply complex input with real input */
+    out1 = ((q63_t) inA1 * inB1) >> 32;
+    out2 = ((q63_t) inA2 * inB1) >> 32;
+    out3 = ((q63_t) inA3 * inB2) >> 32;
+    out4 = ((q63_t) inA4 * inB2) >> 32;
+
+    /* sature the result */
+    out1 = __SSAT(out1, 31);
+    out2 = __SSAT(out2, 31);
+    out3 = __SSAT(out3, 31);
+    out4 = __SSAT(out4, 31);
+
+    /* get result in 1.31 format */
+    out1 = out1 << 1;
+    out2 = out2 << 1;
+    out3 = out3 << 1;
+    out4 = out4 << 1;
+
+    /* store the result to destination buffer */
+    *pCmplxDst++ = out1;
+    *pCmplxDst++ = out2;
+    *pCmplxDst++ = out3;
+    *pCmplxDst++ = out4;
+
+    /* 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;
+
+  while(blkCnt > 0u)
+  {
+    /* C[2 * i] = A[2 * i] * B[i].            */
+    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
+    /* read real input from complex input buffer */
+    inA1 = *pSrcCmplx++;
+    inA2 = *pSrcCmplx++;
+    /* read input from real input bufer */
+    inB1 = *pSrcReal++;
+
+    /* multiply complex input with real input */
+    out1 = ((q63_t) inA1 * inB1) >> 32;
+    out2 = ((q63_t) inA2 * inB1) >> 32;
+
+    /* sature the result */
+    out1 = __SSAT(out1, 31);
+    out2 = __SSAT(out2, 31);
+
+    /* get result in 1.31 format */
+    out1 = out1 << 1;
+    out2 = out2 << 1;
+
+    /* store the result to destination buffer */
+    *pCmplxDst++ = out1;
+    *pCmplxDst++ = out2;
+
+    /* Decrement the numSamples loop counter */
+    blkCnt--;
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  while(numSamples > 0u)
+  {
+    /* realOut = realA * realB.            */
+    /* imagReal = imagA * realB.               */
+    inA1 = *pSrcReal++;
+    /* store the result in the destination buffer. */
+    *pCmplxDst++ =
+      (q31_t) clip_q63_to_q31(((q63_t) * pSrcCmplx++ * inA1) >> 31);
+    *pCmplxDst++ =
+      (q31_t) clip_q63_to_q31(((q63_t) * pSrcCmplx++ * inA1) >> 31);
+
+    /* Decrement the numSamples loop counter */
+    numSamples--;
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of CmplxByRealMult group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c

@@ -0,0 +1,79 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_pid_init_f32.c    
+*    
+* Description:	Floating-point PID Control initialization function    
+*				   
+*    
+* 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"
+
+ /**    
+ * @addtogroup PID    
+ * @{    
+ */
+
+/**    
+ * @brief  Initialization function for the floating-point PID Control.   
+ * @param[in,out] *S points to an instance of the PID structure.   
+ * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state & 1 = reset the state.   
+ * @return none.   
+ * \par Description:   
+ * \par    
+ * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n   
+ * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>    
+ * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)    
+ * also sets the state variables to all zeros.    
+ */
+
+void arm_pid_init_f32(
+  arm_pid_instance_f32 * S,
+  int32_t resetStateFlag)
+{
+
+  /* Derived coefficient A0 */
+  S->A0 = S->Kp + S->Ki + S->Kd;
+
+  /* Derived coefficient A1 */
+  S->A1 = (-S->Kp) - ((float32_t) 2.0 * S->Kd);
+
+  /* Derived coefficient A2 */
+  S->A2 = S->Kd;
+
+  /* Check whether state needs reset or not */
+  if(resetStateFlag)
+  {
+    /* Clear the state buffer.  The size will be always 3 samples */
+    memset(S->state, 0, 3u * sizeof(float32_t));
+  }
+
+}
+
+/**    
+ * @} end of PID group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c

@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_pid_init_q15.c    
+*    
+* Description:	Q15 PID Control initialization function    
+*    
+* 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"
+
+ /**    
+ * @addtogroup PID    
+ * @{    
+ */
+
+/**    
+ * @details    
+ * @param[in,out] *S points to an instance of the Q15 PID structure.    
+ * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.    
+ * @return none.    
+ * \par Description:   
+ * \par    
+ * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n   
+ * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>    
+ * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)    
+ * also sets the state variables to all zeros.    
+ */
+
+void arm_pid_init_q15(
+  arm_pid_instance_q15 * S,
+  int32_t resetStateFlag)
+{
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  /* Derived coefficient A0 */
+  S->A0 = __QADD16(__QADD16(S->Kp, S->Ki), S->Kd);
+
+  /* Derived coefficients and pack into A1 */
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+  S->A1 = __PKHBT(-__QADD16(__QADD16(S->Kd, S->Kd), S->Kp), S->Kd, 16);
+
+#else
+
+  S->A1 = __PKHBT(S->Kd, -__QADD16(__QADD16(S->Kd, S->Kd), S->Kp), 16);
+
+#endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+  /* Check whether state needs reset or not */
+  if(resetStateFlag)
+  {
+    /* Clear the state buffer.  The size will be always 3 samples */
+    memset(S->state, 0, 3u * sizeof(q15_t));
+  }
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  q31_t temp;                                    /*to store the sum */
+
+  /* Derived coefficient A0 */
+  temp = S->Kp + S->Ki + S->Kd;
+  S->A0 = (q15_t) __SSAT(temp, 16);
+
+  /* Derived coefficients and pack into A1 */
+  temp = -(S->Kd + S->Kd + S->Kp);
+  S->A1 = (q15_t) __SSAT(temp, 16);
+  S->A2 = S->Kd;
+
+
+
+  /* Check whether state needs reset or not */
+  if(resetStateFlag)
+  {
+    /* Clear the state buffer.  The size will be always 3 samples */
+    memset(S->state, 0, 3u * sizeof(q15_t));
+  }
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+}
+
+/**    
+ * @} end of PID group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c

@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_pid_init_q31.c    
+*    
+* Description:	Q31 PID Control initialization function     
+*    
+* 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"
+
+ /**    
+ * @addtogroup PID    
+ * @{    
+ */
+
+/**    
+ * @brief  Initialization function for the Q31 PID Control.   
+ * @param[in,out] *S points to an instance of the Q31 PID structure.   
+ * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.   
+ * @return none.    
+ * \par Description:   
+ * \par    
+ * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n   
+ * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>    
+ * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)    
+ * also sets the state variables to all zeros.    
+ */
+
+void arm_pid_init_q31(
+  arm_pid_instance_q31 * S,
+  int32_t resetStateFlag)
+{
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  /* Derived coefficient A0 */
+  S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd);
+
+  /* Derived coefficient A1 */
+  S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp);
+
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  q31_t temp;
+
+  /* Derived coefficient A0 */
+  temp = clip_q63_to_q31((q63_t) S->Kp + S->Ki);
+  S->A0 = clip_q63_to_q31((q63_t) temp + S->Kd);
+
+  /* Derived coefficient A1 */
+  temp = clip_q63_to_q31((q63_t) S->Kd + S->Kd);
+  S->A1 = -clip_q63_to_q31((q63_t) temp + S->Kp);
+
+#endif /* #ifndef ARM_MATH_CM0 */
+
+  /* Derived coefficient A2 */
+  S->A2 = S->Kd;
+
+  /* Check whether state needs reset or not */
+  if(resetStateFlag)
+  {
+    /* Clear the state buffer.  The size will be always 3 samples */
+    memset(S->state, 0, 3u * sizeof(q31_t));
+  }
+
+}
+
+/**    
+ * @} end of PID group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c

@@ -0,0 +1,57 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_pid_reset_f32.c    
+*    
+* Description:	Floating-point PID Control reset function   
+*    
+* 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"
+
+ /**    
+ * @addtogroup PID    
+ * @{    
+ */
+
+/**    
+* @brief  Reset function for the floating-point PID Control.   
+* @param[in] *S	Instance pointer of PID control data structure.   
+* @return none.    
+* \par Description:   
+* The function resets the state buffer to zeros.    
+*/
+void arm_pid_reset_f32(
+  arm_pid_instance_f32 * S)
+{
+
+  /* Clear the state buffer.  The size will be always 3 samples */
+  memset(S->state, 0, 3u * sizeof(float32_t));
+}
+
+/**    
+ * @} end of PID group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c

@@ -0,0 +1,56 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_pid_reset_q15.c    
+*    
+* Description:	Q15 PID Control reset function   
+*    
+* 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"
+
+ /**    
+ * @addtogroup PID    
+ * @{    
+ */
+
+/**    
+* @brief  Reset function for the Q15 PID Control.   
+* @param[in] *S		Instance pointer of PID control data structure.   
+* @return none.    
+* \par Description:   
+* The function resets the state buffer to zeros.    
+*/
+void arm_pid_reset_q15(
+  arm_pid_instance_q15 * S)
+{
+  /* Reset state to zero, The size will be always 3 samples */
+  memset(S->state, 0, 3u * sizeof(q15_t));
+}
+
+/**    
+ * @} end of PID group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q31.c

@@ -0,0 +1,57 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_pid_reset_q31.c    
+*    
+* Description:	Q31 PID Control reset function   
+*    
+* 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"
+
+ /**    
+ * @addtogroup PID    
+ * @{    
+ */
+
+/**    
+* @brief  Reset function for the Q31 PID Control.   
+* @param[in] *S	Instance pointer of PID control data structure.   
+* @return none.    
+* \par Description:   
+* The function resets the state buffer to zeros.    
+*/
+void arm_pid_reset_q31(
+  arm_pid_instance_q31 * S)
+{
+
+  /* Clear the state buffer.  The size will be always 3 samples */
+  memset(S->state, 0, 3u * sizeof(q31_t));
+}
+
+/**    
+ * @} end of PID group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_f32.c

@@ -0,0 +1,428 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sin_cos_f32.c    
+*    
+* Description:	Sine and Cosine calculation for floating-point values.   
+*    
+* 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 groupController    
+ */
+
+/**    
+ * @defgroup SinCos Sine Cosine   
+ *    
+ * Computes the trigonometric sine and cosine values using a combination of table lookup   
+ * and linear interpolation.     
+ * There are separate functions for Q31 and floating-point data types.   
+ * The input to the floating-point version is in degrees while the   
+ * fixed-point Q31 have a scaled input with the range   
+ * [-1 0.9999] mapping to [-180 179] degrees.   
+ *   
+ * The implementation is based on table lookup using 360 values together with linear interpolation.   
+ * The steps used are:   
+ *  -# Calculation of the nearest integer table index.   
+ *  -# Compute the fractional portion (fract) of the input.   
+ *  -# Fetch the value corresponding to \c index from sine table to \c y0 and also value from \c index+1 to \c y1.      
+ *  -# Sine value is computed as <code> *psinVal = y0 + (fract * (y1 - y0))</code>.    
+ *  -# Fetch the value corresponding to \c index from cosine table to \c y0 and also value from \c index+1 to \c y1.      
+ *  -# Cosine value is computed as <code> *pcosVal = y0 + (fract * (y1 - y0))</code>.    
+ */
+
+ /**    
+ * @addtogroup SinCos    
+ * @{    
+ */
+
+
+/**    
+* \par    
+* Cosine Table is generated from following loop    
+* <pre>for(i = 0; i < 360; i++)    
+* {    
+*    cosTable[i]= cos((i-180) * PI/180.0);    
+* } </pre>   
+*/
+
+static const float32_t cosTable[360] = {
+  -0.999847695156391270f, -0.999390827019095760f, -0.998629534754573830f,
+  -0.997564050259824200f, -0.996194698091745550f, -0.994521895368273290f,
+  -0.992546151641321980f, -0.990268068741570250f,
+  -0.987688340595137660f, -0.984807753012208020f, -0.981627183447663980f,
+  -0.978147600733805690f, -0.974370064785235250f, -0.970295726275996470f,
+  -0.965925826289068200f, -0.961261695938318670f,
+  -0.956304755963035440f, -0.951056516295153530f, -0.945518575599316740f,
+  -0.939692620785908320f, -0.933580426497201740f, -0.927183854566787310f,
+  -0.920504853452440150f, -0.913545457642600760f,
+  -0.906307787036649940f, -0.898794046299167040f, -0.891006524188367790f,
+  -0.882947592858926770f, -0.874619707139395740f, -0.866025403784438710f,
+  -0.857167300702112220f, -0.848048096156425960f,
+  -0.838670567945424160f, -0.829037572555041620f, -0.819152044288991580f,
+  -0.809016994374947340f, -0.798635510047292940f, -0.788010753606721900f,
+  -0.777145961456970680f, -0.766044443118977900f,
+  -0.754709580222772010f, -0.743144825477394130f, -0.731353701619170460f,
+  -0.719339800338651300f, -0.707106781186547460f, -0.694658370458997030f,
+  -0.681998360062498370f, -0.669130606358858240f,
+  -0.656059028990507500f, -0.642787609686539360f, -0.629320391049837280f,
+  -0.615661475325658290f, -0.601815023152048380f, -0.587785252292473030f,
+  -0.573576436351045830f, -0.559192903470746680f,
+  -0.544639035015027080f, -0.529919264233204790f, -0.515038074910054270f,
+  -0.499999999999999780f, -0.484809620246337000f, -0.469471562785890530f,
+  -0.453990499739546750f, -0.438371146789077510f,
+  -0.422618261740699330f, -0.406736643075800100f, -0.390731128489273600f,
+  -0.374606593415912070f, -0.358367949545300270f, -0.342020143325668710f,
+  -0.325568154457156420f, -0.309016994374947340f,
+  -0.292371704722736660f, -0.275637355816999050f, -0.258819045102520850f,
+  -0.241921895599667790f, -0.224951054343864810f, -0.207911690817759120f,
+  -0.190808995376544800f, -0.173648177666930300f,
+  -0.156434465040231040f, -0.139173100960065350f, -0.121869343405147370f,
+  -0.104528463267653330f, -0.087155742747658235f, -0.069756473744125330f,
+  -0.052335956242943620f, -0.034899496702500733f,
+  -0.017452406437283477f, 0.000000000000000061f, 0.017452406437283376f,
+  0.034899496702501080f, 0.052335956242943966f, 0.069756473744125455f,
+  0.087155742747658138f, 0.104528463267653460f,
+  0.121869343405147490f, 0.139173100960065690f, 0.156434465040230920f,
+  0.173648177666930410f, 0.190808995376544920f, 0.207911690817759450f,
+  0.224951054343864920f, 0.241921895599667900f,
+  0.258819045102520740f, 0.275637355816999160f, 0.292371704722736770f,
+  0.309016994374947450f, 0.325568154457156760f, 0.342020143325668820f,
+  0.358367949545300380f, 0.374606593415911960f,
+  0.390731128489273940f, 0.406736643075800210f, 0.422618261740699440f,
+  0.438371146789077460f, 0.453990499739546860f, 0.469471562785890860f,
+  0.484809620246337110f, 0.500000000000000110f,
+  0.515038074910054380f, 0.529919264233204900f, 0.544639035015027200f,
+  0.559192903470746790f, 0.573576436351046050f, 0.587785252292473140f,
+  0.601815023152048270f, 0.615661475325658290f,
+  0.629320391049837500f, 0.642787609686539360f, 0.656059028990507280f,
+  0.669130606358858240f, 0.681998360062498480f, 0.694658370458997370f,
+  0.707106781186547570f, 0.719339800338651190f,
+  0.731353701619170570f, 0.743144825477394240f, 0.754709580222772010f,
+  0.766044443118978010f, 0.777145961456970900f, 0.788010753606722010f,
+  0.798635510047292830f, 0.809016994374947450f,
+  0.819152044288991800f, 0.829037572555041620f, 0.838670567945424050f,
+  0.848048096156425960f, 0.857167300702112330f, 0.866025403784438710f,
+  0.874619707139395740f, 0.882947592858926990f,
+  0.891006524188367900f, 0.898794046299167040f, 0.906307787036649940f,
+  0.913545457642600870f, 0.920504853452440370f, 0.927183854566787420f,
+  0.933580426497201740f, 0.939692620785908430f,
+  0.945518575599316850f, 0.951056516295153530f, 0.956304755963035440f,
+  0.961261695938318890f, 0.965925826289068310f, 0.970295726275996470f,
+  0.974370064785235250f, 0.978147600733805690f,
+  0.981627183447663980f, 0.984807753012208020f, 0.987688340595137770f,
+  0.990268068741570360f, 0.992546151641321980f, 0.994521895368273290f,
+  0.996194698091745550f, 0.997564050259824200f,
+  0.998629534754573830f, 0.999390827019095760f, 0.999847695156391270f,
+  1.000000000000000000f, 0.999847695156391270f, 0.999390827019095760f,
+  0.998629534754573830f, 0.997564050259824200f,
+  0.996194698091745550f, 0.994521895368273290f, 0.992546151641321980f,
+  0.990268068741570360f, 0.987688340595137770f, 0.984807753012208020f,
+  0.981627183447663980f, 0.978147600733805690f,
+  0.974370064785235250f, 0.970295726275996470f, 0.965925826289068310f,
+  0.961261695938318890f, 0.956304755963035440f, 0.951056516295153530f,
+  0.945518575599316850f, 0.939692620785908430f,
+  0.933580426497201740f, 0.927183854566787420f, 0.920504853452440370f,
+  0.913545457642600870f, 0.906307787036649940f, 0.898794046299167040f,
+  0.891006524188367900f, 0.882947592858926990f,
+  0.874619707139395740f, 0.866025403784438710f, 0.857167300702112330f,
+  0.848048096156425960f, 0.838670567945424050f, 0.829037572555041620f,
+  0.819152044288991800f, 0.809016994374947450f,
+  0.798635510047292830f, 0.788010753606722010f, 0.777145961456970900f,
+  0.766044443118978010f, 0.754709580222772010f, 0.743144825477394240f,
+  0.731353701619170570f, 0.719339800338651190f,
+  0.707106781186547570f, 0.694658370458997370f, 0.681998360062498480f,
+  0.669130606358858240f, 0.656059028990507280f, 0.642787609686539360f,
+  0.629320391049837500f, 0.615661475325658290f,
+  0.601815023152048270f, 0.587785252292473140f, 0.573576436351046050f,
+  0.559192903470746790f, 0.544639035015027200f, 0.529919264233204900f,
+  0.515038074910054380f, 0.500000000000000110f,
+  0.484809620246337110f, 0.469471562785890860f, 0.453990499739546860f,
+  0.438371146789077460f, 0.422618261740699440f, 0.406736643075800210f,
+  0.390731128489273940f, 0.374606593415911960f,
+  0.358367949545300380f, 0.342020143325668820f, 0.325568154457156760f,
+  0.309016994374947450f, 0.292371704722736770f, 0.275637355816999160f,
+  0.258819045102520740f, 0.241921895599667900f,
+  0.224951054343864920f, 0.207911690817759450f, 0.190808995376544920f,
+  0.173648177666930410f, 0.156434465040230920f, 0.139173100960065690f,
+  0.121869343405147490f, 0.104528463267653460f,
+  0.087155742747658138f, 0.069756473744125455f, 0.052335956242943966f,
+  0.034899496702501080f, 0.017452406437283376f, 0.000000000000000061f,
+  -0.017452406437283477f, -0.034899496702500733f,
+  -0.052335956242943620f, -0.069756473744125330f, -0.087155742747658235f,
+  -0.104528463267653330f, -0.121869343405147370f, -0.139173100960065350f,
+  -0.156434465040231040f, -0.173648177666930300f,
+  -0.190808995376544800f, -0.207911690817759120f, -0.224951054343864810f,
+  -0.241921895599667790f, -0.258819045102520850f, -0.275637355816999050f,
+  -0.292371704722736660f, -0.309016994374947340f,
+  -0.325568154457156420f, -0.342020143325668710f, -0.358367949545300270f,
+  -0.374606593415912070f, -0.390731128489273600f, -0.406736643075800100f,
+  -0.422618261740699330f, -0.438371146789077510f,
+  -0.453990499739546750f, -0.469471562785890530f, -0.484809620246337000f,
+  -0.499999999999999780f, -0.515038074910054270f, -0.529919264233204790f,
+  -0.544639035015027080f, -0.559192903470746680f,
+  -0.573576436351045830f, -0.587785252292473030f, -0.601815023152048380f,
+  -0.615661475325658290f, -0.629320391049837280f, -0.642787609686539360f,
+  -0.656059028990507500f, -0.669130606358858240f,
+  -0.681998360062498370f, -0.694658370458997030f, -0.707106781186547460f,
+  -0.719339800338651300f, -0.731353701619170460f, -0.743144825477394130f,
+  -0.754709580222772010f, -0.766044443118977900f,
+  -0.777145961456970680f, -0.788010753606721900f, -0.798635510047292940f,
+  -0.809016994374947340f, -0.819152044288991580f, -0.829037572555041620f,
+  -0.838670567945424160f, -0.848048096156425960f,
+  -0.857167300702112220f, -0.866025403784438710f, -0.874619707139395740f,
+  -0.882947592858926770f, -0.891006524188367790f, -0.898794046299167040f,
+  -0.906307787036649940f, -0.913545457642600760f,
+  -0.920504853452440150f, -0.927183854566787310f, -0.933580426497201740f,
+  -0.939692620785908320f, -0.945518575599316740f, -0.951056516295153530f,
+  -0.956304755963035440f, -0.961261695938318670f,
+  -0.965925826289068200f, -0.970295726275996470f, -0.974370064785235250f,
+  -0.978147600733805690f, -0.981627183447663980f, -0.984807753012208020f,
+  -0.987688340595137660f, -0.990268068741570250f,
+  -0.992546151641321980f, -0.994521895368273290f, -0.996194698091745550f,
+  -0.997564050259824200f, -0.998629534754573830f, -0.999390827019095760f,
+  -0.999847695156391270f, -1.000000000000000000f
+};
+
+/**    
+* \par    
+* Sine Table is generated from following loop    
+* <pre>for(i = 0; i < 360; i++)    
+* {    
+*    sinTable[i]= sin((i-180) * PI/180.0);    
+* } </pre>    
+*/
+
+
+static const float32_t sinTable[360] = {
+  -0.017452406437283439f, -0.034899496702500699f, -0.052335956242943807f,
+  -0.069756473744125524f, -0.087155742747658638f, -0.104528463267653730f,
+  -0.121869343405147550f, -0.139173100960065740f,
+  -0.156434465040230980f, -0.173648177666930280f, -0.190808995376544970f,
+  -0.207911690817759310f, -0.224951054343864780f, -0.241921895599667730f,
+  -0.258819045102521020f, -0.275637355816999660f,
+  -0.292371704722737050f, -0.309016994374947510f, -0.325568154457156980f,
+  -0.342020143325668880f, -0.358367949545300210f, -0.374606593415912240f,
+  -0.390731128489274160f, -0.406736643075800430f,
+  -0.422618261740699500f, -0.438371146789077290f, -0.453990499739546860f,
+  -0.469471562785891080f, -0.484809620246337170f, -0.499999999999999940f,
+  -0.515038074910054380f, -0.529919264233204900f,
+  -0.544639035015026860f, -0.559192903470746900f, -0.573576436351046380f,
+  -0.587785252292473250f, -0.601815023152048160f, -0.615661475325658400f,
+  -0.629320391049837720f, -0.642787609686539470f,
+  -0.656059028990507280f, -0.669130606358858350f, -0.681998360062498590f,
+  -0.694658370458997140f, -0.707106781186547570f, -0.719339800338651410f,
+  -0.731353701619170570f, -0.743144825477394240f,
+  -0.754709580222771790f, -0.766044443118978010f, -0.777145961456971010f,
+  -0.788010753606722010f, -0.798635510047292720f, -0.809016994374947450f,
+  -0.819152044288992020f, -0.829037572555041740f,
+  -0.838670567945424050f, -0.848048096156426070f, -0.857167300702112330f,
+  -0.866025403784438710f, -0.874619707139395850f, -0.882947592858927100f,
+  -0.891006524188367900f, -0.898794046299166930f,
+  -0.906307787036650050f, -0.913545457642600980f, -0.920504853452440370f,
+  -0.927183854566787420f, -0.933580426497201740f, -0.939692620785908430f,
+  -0.945518575599316850f, -0.951056516295153640f,
+  -0.956304755963035550f, -0.961261695938318890f, -0.965925826289068310f,
+  -0.970295726275996470f, -0.974370064785235250f, -0.978147600733805690f,
+  -0.981627183447663980f, -0.984807753012208020f,
+  -0.987688340595137660f, -0.990268068741570360f, -0.992546151641322090f,
+  -0.994521895368273400f, -0.996194698091745550f, -0.997564050259824200f,
+  -0.998629534754573830f, -0.999390827019095760f,
+  -0.999847695156391270f, -1.000000000000000000f, -0.999847695156391270f,
+  -0.999390827019095760f, -0.998629534754573830f, -0.997564050259824200f,
+  -0.996194698091745550f, -0.994521895368273290f,
+  -0.992546151641321980f, -0.990268068741570250f, -0.987688340595137770f,
+  -0.984807753012208020f, -0.981627183447663980f, -0.978147600733805580f,
+  -0.974370064785235250f, -0.970295726275996470f,
+  -0.965925826289068310f, -0.961261695938318890f, -0.956304755963035440f,
+  -0.951056516295153530f, -0.945518575599316740f, -0.939692620785908320f,
+  -0.933580426497201740f, -0.927183854566787420f,
+  -0.920504853452440260f, -0.913545457642600870f, -0.906307787036649940f,
+  -0.898794046299167040f, -0.891006524188367790f, -0.882947592858926880f,
+  -0.874619707139395740f, -0.866025403784438600f,
+  -0.857167300702112220f, -0.848048096156426070f, -0.838670567945423940f,
+  -0.829037572555041740f, -0.819152044288991800f, -0.809016994374947450f,
+  -0.798635510047292830f, -0.788010753606722010f,
+  -0.777145961456970790f, -0.766044443118978010f, -0.754709580222772010f,
+  -0.743144825477394240f, -0.731353701619170460f, -0.719339800338651080f,
+  -0.707106781186547460f, -0.694658370458997250f,
+  -0.681998360062498480f, -0.669130606358858240f, -0.656059028990507160f,
+  -0.642787609686539250f, -0.629320391049837390f, -0.615661475325658180f,
+  -0.601815023152048270f, -0.587785252292473140f,
+  -0.573576436351046050f, -0.559192903470746900f, -0.544639035015027080f,
+  -0.529919264233204900f, -0.515038074910054160f, -0.499999999999999940f,
+  -0.484809620246337060f, -0.469471562785890810f,
+  -0.453990499739546750f, -0.438371146789077400f, -0.422618261740699440f,
+  -0.406736643075800150f, -0.390731128489273720f, -0.374606593415912010f,
+  -0.358367949545300270f, -0.342020143325668710f,
+  -0.325568154457156640f, -0.309016994374947400f, -0.292371704722736770f,
+  -0.275637355816999160f, -0.258819045102520740f, -0.241921895599667730f,
+  -0.224951054343865000f, -0.207911690817759310f,
+  -0.190808995376544800f, -0.173648177666930330f, -0.156434465040230870f,
+  -0.139173100960065440f, -0.121869343405147480f, -0.104528463267653460f,
+  -0.087155742747658166f, -0.069756473744125302f,
+  -0.052335956242943828f, -0.034899496702500969f, -0.017452406437283512f,
+  0.000000000000000000f, 0.017452406437283512f, 0.034899496702500969f,
+  0.052335956242943828f, 0.069756473744125302f,
+  0.087155742747658166f, 0.104528463267653460f, 0.121869343405147480f,
+  0.139173100960065440f, 0.156434465040230870f, 0.173648177666930330f,
+  0.190808995376544800f, 0.207911690817759310f,
+  0.224951054343865000f, 0.241921895599667730f, 0.258819045102520740f,
+  0.275637355816999160f, 0.292371704722736770f, 0.309016994374947400f,
+  0.325568154457156640f, 0.342020143325668710f,
+  0.358367949545300270f, 0.374606593415912010f, 0.390731128489273720f,
+  0.406736643075800150f, 0.422618261740699440f, 0.438371146789077400f,
+  0.453990499739546750f, 0.469471562785890810f,
+  0.484809620246337060f, 0.499999999999999940f, 0.515038074910054160f,
+  0.529919264233204900f, 0.544639035015027080f, 0.559192903470746900f,
+  0.573576436351046050f, 0.587785252292473140f,
+  0.601815023152048270f, 0.615661475325658180f, 0.629320391049837390f,
+  0.642787609686539250f, 0.656059028990507160f, 0.669130606358858240f,
+  0.681998360062498480f, 0.694658370458997250f,
+  0.707106781186547460f, 0.719339800338651080f, 0.731353701619170460f,
+  0.743144825477394240f, 0.754709580222772010f, 0.766044443118978010f,
+  0.777145961456970790f, 0.788010753606722010f,
+  0.798635510047292830f, 0.809016994374947450f, 0.819152044288991800f,
+  0.829037572555041740f, 0.838670567945423940f, 0.848048096156426070f,
+  0.857167300702112220f, 0.866025403784438600f,
+  0.874619707139395740f, 0.882947592858926880f, 0.891006524188367790f,
+  0.898794046299167040f, 0.906307787036649940f, 0.913545457642600870f,
+  0.920504853452440260f, 0.927183854566787420f,
+  0.933580426497201740f, 0.939692620785908320f, 0.945518575599316740f,
+  0.951056516295153530f, 0.956304755963035440f, 0.961261695938318890f,
+  0.965925826289068310f, 0.970295726275996470f,
+  0.974370064785235250f, 0.978147600733805580f, 0.981627183447663980f,
+  0.984807753012208020f, 0.987688340595137770f, 0.990268068741570250f,
+  0.992546151641321980f, 0.994521895368273290f,
+  0.996194698091745550f, 0.997564050259824200f, 0.998629534754573830f,
+  0.999390827019095760f, 0.999847695156391270f, 1.000000000000000000f,
+  0.999847695156391270f, 0.999390827019095760f,
+  0.998629534754573830f, 0.997564050259824200f, 0.996194698091745550f,
+  0.994521895368273400f, 0.992546151641322090f, 0.990268068741570360f,
+  0.987688340595137660f, 0.984807753012208020f,
+  0.981627183447663980f, 0.978147600733805690f, 0.974370064785235250f,
+  0.970295726275996470f, 0.965925826289068310f, 0.961261695938318890f,
+  0.956304755963035550f, 0.951056516295153640f,
+  0.945518575599316850f, 0.939692620785908430f, 0.933580426497201740f,
+  0.927183854566787420f, 0.920504853452440370f, 0.913545457642600980f,
+  0.906307787036650050f, 0.898794046299166930f,
+  0.891006524188367900f, 0.882947592858927100f, 0.874619707139395850f,
+  0.866025403784438710f, 0.857167300702112330f, 0.848048096156426070f,
+  0.838670567945424050f, 0.829037572555041740f,
+  0.819152044288992020f, 0.809016994374947450f, 0.798635510047292720f,
+  0.788010753606722010f, 0.777145961456971010f, 0.766044443118978010f,
+  0.754709580222771790f, 0.743144825477394240f,
+  0.731353701619170570f, 0.719339800338651410f, 0.707106781186547570f,
+  0.694658370458997140f, 0.681998360062498590f, 0.669130606358858350f,
+  0.656059028990507280f, 0.642787609686539470f,
+  0.629320391049837720f, 0.615661475325658400f, 0.601815023152048160f,
+  0.587785252292473250f, 0.573576436351046380f, 0.559192903470746900f,
+  0.544639035015026860f, 0.529919264233204900f,
+  0.515038074910054380f, 0.499999999999999940f, 0.484809620246337170f,
+  0.469471562785891080f, 0.453990499739546860f, 0.438371146789077290f,
+  0.422618261740699500f, 0.406736643075800430f,
+  0.390731128489274160f, 0.374606593415912240f, 0.358367949545300210f,
+  0.342020143325668880f, 0.325568154457156980f, 0.309016994374947510f,
+  0.292371704722737050f, 0.275637355816999660f,
+  0.258819045102521020f, 0.241921895599667730f, 0.224951054343864780f,
+  0.207911690817759310f, 0.190808995376544970f, 0.173648177666930280f,
+  0.156434465040230980f, 0.139173100960065740f,
+  0.121869343405147550f, 0.104528463267653730f, 0.087155742747658638f,
+  0.069756473744125524f, 0.052335956242943807f, 0.034899496702500699f,
+  0.017452406437283439f, 0.000000000000000122f
+};
+
+
+/**    
+ * @brief  Floating-point sin_cos function.   
+ * @param[in]  theta    input value in degrees    
+ * @param[out] *pSinVal points to the processed sine output.    
+ * @param[out] *pCosVal points to the processed cos output.    
+ * @return none.   
+ */
+
+
+void arm_sin_cos_f32(
+  float32_t theta,
+  float32_t * pSinVal,
+  float32_t * pCosVal)
+{
+  int32_t i;                                     /* Index for reading nearwst output values */
+  float32_t x1 = -179.0f;                        /* Initial input value */
+  float32_t y0, y1;                              /* nearest output values */
+  float32_t y2, y3;
+  float32_t fract;                               /* fractional part of input */
+
+  /* Calculation of fractional part */
+  if(theta > 0.0f)
+  {
+    fract = theta - (float32_t) ((int32_t) theta);
+  }
+  else
+  {
+    fract = (theta - (float32_t) ((int32_t) theta)) + 1.0f;
+  }
+
+  /* index calculation for reading nearest output values */
+  i = (uint32_t) (theta - x1);
+
+  /* Checking min and max index of table */
+  if(i < 0)
+  {
+    i = 0;
+  }
+  else if(i >= 359)
+  {
+    i = 358;
+  }
+
+  /* reading nearest sine output values */
+  y0 = sinTable[i];
+  y1 = sinTable[i + 1u];
+
+  /* reading nearest cosine output values */
+  y2 = cosTable[i];
+  y3 = cosTable[i + 1u];
+
+  y1 = y1 - y0;
+  y3 = y3 - y2;
+
+  y1 = fract * y1;
+  y3 = fract * y3;
+
+  /* Calculation of sine value */
+  *pSinVal = y0 + y1;
+
+  /* Calculation of cosine value */
+  *pCosVal = y2 + y3;
+
+}
+
+/**    
+ * @} end of SinCos group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_q31.c

@@ -0,0 +1,324 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sin_cos_q31.c    
+*    
+* Description:	Cosine & Sine calculation for Q31 values.   
+*    
+* 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 groupController    
+ */
+
+ /**    
+ * @addtogroup SinCos    
+ * @{    
+ */
+
+/**    
+* \par    
+* Sine Table is generated from following loop    
+* <pre>for(i = 0; i < 360; i++)    
+* {    
+*    sinTable[i]= sin((i-180) * PI/180.0);    
+* } </pre>   
+* Convert above coefficients to fixed point 1.31 format.    
+*/
+
+static const int32_t sinTableQ31[360] = {
+
+  0x0, 0xfdc41e9b, 0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2,
+  0xf06695da,
+  0xee2f9369, 0xebf9f498, 0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9,
+  0xe108b40d, 0xdedf047d,
+  0xdcb7ea46, 0xda939061, 0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0,
+  0xd00ce422, 0xcdfc85bb,
+  0xcbf00dbe, 0xc9e7a512, 0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224,
+  0xc0000000, 0xbe133b7c,
+  0xbc2b9b05, 0xba4944a2, 0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af,
+  0xb1320139, 0xaf726def,
+  0xadb922b7, 0xac0641fb, 0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666,
+  0xa3ecac65, 0xa263007d,
+  0xa0e0a15f, 0x9f65ad2d, 0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5,
+  0x98722192, 0x9726069c,
+  0x95e218c9, 0x94a6715d, 0x937328f5, 0x92485786, 0x9126145f, 0x900c7621,
+  0x8efb92c2, 0x8df37f8b,
+  0x8cf45113, 0x8bfe1b3f, 0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4,
+  0x87b826f7, 0x86f93f50,
+  0x8643c7b3, 0x8597ce46, 0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b,
+  0x82cc0f36, 0x825a0a5b,
+  0x81f1d1ce, 0x81936daf, 0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130,
+  0x804fd23a, 0x802ce84c,
+  0x8013f61d, 0x8004fda0, 0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c,
+  0x804fd23a, 0x807cb130,
+  0x80b381ac, 0x80f43f69, 0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b,
+  0x82cc0f36, 0x8347d77b,
+  0x83cd5982, 0x845c8ae3, 0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50,
+  0x87b826f7, 0x88806fc4,
+  0x89520a1a, 0x8a2ce59f, 0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b,
+  0x8efb92c2, 0x900c7621,
+  0x9126145f, 0x92485786, 0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c,
+  0x98722192, 0x99c64fc5,
+  0x9b2276b0, 0x9c867b2c, 0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d,
+  0xa3ecac65, 0xa57d8666,
+  0xa7156f3c, 0xa8b4471a, 0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def,
+  0xb1320139, 0xb2f7b9af,
+  0xb4c373ee, 0xb6950c1e, 0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c,
+  0xc0000000, 0xc1f1c224,
+  0xc3e85b18, 0xc5e3a3a9, 0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb,
+  0xd00ce422, 0xd220ffc0,
+  0xd438af17, 0xd653c860, 0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d,
+  0xe108b40d, 0xe334cdc9,
+  0xe5632654, 0xe7939223, 0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da,
+  0xf29ecfb2, 0xf4d814a4,
+  0xf7123849, 0xf94d0e2e, 0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632,
+  0x6b2f1d2,
+  0x8edc7b7, 0xb27eb5c, 0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68,
+  0x163a1a7e, 0x186c6ddd,
+  0x1a9cd9ac, 0x1ccb3237, 0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f,
+  0x278dde6e, 0x29ac37a0,
+  0x2bc750e9, 0x2ddf0040, 0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee,
+  0x381c8bb5, 0x3a1c5c57,
+  0x3c17a4e8, 0x3e0e3ddc, 0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e,
+  0x4793a210, 0x496af3e2,
+  0x4b3c8c12, 0x4d084651, 0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05,
+  0x55a6125c, 0x574bb8e6,
+  0x58ea90c4, 0x5a82799a, 0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3,
+  0x620dbe8b, 0x637984d4,
+  0x64dd8950, 0x6639b03b, 0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3,
+  0x6c8cd70b, 0x6db7a87a,
+  0x6ed9eba1, 0x6ff389df, 0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1,
+  0x74ef0ebc, 0x75d31a61,
+  0x76adf5e6, 0x777f903c, 0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba,
+  0x7b0a9f8d, 0x7ba3751d,
+  0x7c32a67e, 0x7cb82885, 0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251,
+  0x7ec11aa5, 0x7f0bc097,
+  0x7f4c7e54, 0x7f834ed0, 0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260,
+  0x7fffffff, 0x7ffb0260,
+  0x7fec09e3, 0x7fd317b4, 0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097,
+  0x7ec11aa5, 0x7e6c9251,
+  0x7e0e2e32, 0x7da5f5a5, 0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d,
+  0x7b0a9f8d, 0x7a6831ba,
+  0x79bc384d, 0x7906c0b0, 0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61,
+  0x74ef0ebc, 0x7401e4c1,
+  0x730baeed, 0x720c8075, 0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a,
+  0x6c8cd70b, 0x6b598ea3,
+  0x6a1de737, 0x68d9f964, 0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4,
+  0x620dbe8b, 0x609a52d3,
+  0x5f1f5ea1, 0x5d9cff83, 0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6,
+  0x55a6125c, 0x53f9be05,
+  0x5246dd49, 0x508d9211, 0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2,
+  0x4793a210, 0x45b6bb5e,
+  0x43d464fb, 0x41ecc484, 0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57,
+  0x381c8bb5, 0x36185aee,
+  0x340ff242, 0x32037a45, 0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0,
+  0x278dde6e, 0x256c6f9f,
+  0x234815ba, 0x2120fb83, 0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd,
+  0x163a1a7e, 0x14060b68,
+  0x11d06c97, 0xf996a26, 0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2,
+  0x4779632, 0x23be165,
+
+
+};
+
+/**    
+* \par    
+* Cosine Table is generated from following loop    
+* <pre>for(i = 0; i < 360; i++)    
+* {    
+*    cosTable[i]= cos((i-180) * PI/180.0);    
+* } </pre>   
+* \par    
+* Convert above coefficients to fixed point 1.31 format.    
+*/
+static const int32_t cosTableQ31[360] = {
+  0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, 0x804fd23a, 0x807cb130,
+  0x80b381ac, 0x80f43f69,
+  0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, 0x82cc0f36, 0x8347d77b,
+  0x83cd5982, 0x845c8ae3,
+  0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, 0x87b826f7, 0x88806fc4,
+  0x89520a1a, 0x8a2ce59f,
+  0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, 0x8efb92c2, 0x900c7621,
+  0x9126145f, 0x92485786,
+  0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, 0x98722192, 0x99c64fc5,
+  0x9b2276b0, 0x9c867b2c,
+  0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, 0xa3ecac65, 0xa57d8666,
+  0xa7156f3c, 0xa8b4471a,
+  0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, 0xb1320139, 0xb2f7b9af,
+  0xb4c373ee, 0xb6950c1e,
+  0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, 0xc0000000, 0xc1f1c224,
+  0xc3e85b18, 0xc5e3a3a9,
+  0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, 0xd00ce422, 0xd220ffc0,
+  0xd438af17, 0xd653c860,
+  0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, 0xe108b40d, 0xe334cdc9,
+  0xe5632654, 0xe7939223,
+  0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, 0xf29ecfb2, 0xf4d814a4,
+  0xf7123849, 0xf94d0e2e,
+  0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, 0x6b2f1d2, 0x8edc7b7,
+  0xb27eb5c,
+  0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, 0x163a1a7e, 0x186c6ddd,
+  0x1a9cd9ac, 0x1ccb3237,
+  0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, 0x278dde6e, 0x29ac37a0,
+  0x2bc750e9, 0x2ddf0040,
+  0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, 0x381c8bb5, 0x3a1c5c57,
+  0x3c17a4e8, 0x3e0e3ddc,
+  0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, 0x4793a210, 0x496af3e2,
+  0x4b3c8c12, 0x4d084651,
+  0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, 0x55a6125c, 0x574bb8e6,
+  0x58ea90c4, 0x5a82799a,
+  0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, 0x620dbe8b, 0x637984d4,
+  0x64dd8950, 0x6639b03b,
+  0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, 0x6c8cd70b, 0x6db7a87a,
+  0x6ed9eba1, 0x6ff389df,
+  0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, 0x74ef0ebc, 0x75d31a61,
+  0x76adf5e6, 0x777f903c,
+  0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, 0x7b0a9f8d, 0x7ba3751d,
+  0x7c32a67e, 0x7cb82885,
+  0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, 0x7ec11aa5, 0x7f0bc097,
+  0x7f4c7e54, 0x7f834ed0,
+  0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, 0x7fffffff, 0x7ffb0260,
+  0x7fec09e3, 0x7fd317b4,
+  0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, 0x7ec11aa5, 0x7e6c9251,
+  0x7e0e2e32, 0x7da5f5a5,
+  0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, 0x7b0a9f8d, 0x7a6831ba,
+  0x79bc384d, 0x7906c0b0,
+  0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, 0x74ef0ebc, 0x7401e4c1,
+  0x730baeed, 0x720c8075,
+  0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, 0x6c8cd70b, 0x6b598ea3,
+  0x6a1de737, 0x68d9f964,
+  0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, 0x620dbe8b, 0x609a52d3,
+  0x5f1f5ea1, 0x5d9cff83,
+  0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, 0x55a6125c, 0x53f9be05,
+  0x5246dd49, 0x508d9211,
+  0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, 0x4793a210, 0x45b6bb5e,
+  0x43d464fb, 0x41ecc484,
+  0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, 0x381c8bb5, 0x36185aee,
+  0x340ff242, 0x32037a45,
+  0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, 0x278dde6e, 0x256c6f9f,
+  0x234815ba, 0x2120fb83,
+  0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, 0x163a1a7e, 0x14060b68,
+  0x11d06c97, 0xf996a26,
+  0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, 0x4779632, 0x23be165, 0x0,
+  0xfdc41e9b,
+  0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, 0xf06695da,
+  0xee2f9369, 0xebf9f498,
+  0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, 0xe108b40d, 0xdedf047d,
+  0xdcb7ea46, 0xda939061,
+  0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, 0xd00ce422, 0xcdfc85bb,
+  0xcbf00dbe, 0xc9e7a512,
+  0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, 0xc0000000, 0xbe133b7c,
+  0xbc2b9b05, 0xba4944a2,
+  0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, 0xb1320139, 0xaf726def,
+  0xadb922b7, 0xac0641fb,
+  0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, 0xa3ecac65, 0xa263007d,
+  0xa0e0a15f, 0x9f65ad2d,
+  0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, 0x98722192, 0x9726069c,
+  0x95e218c9, 0x94a6715d,
+  0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, 0x8efb92c2, 0x8df37f8b,
+  0x8cf45113, 0x8bfe1b3f,
+  0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, 0x87b826f7, 0x86f93f50,
+  0x8643c7b3, 0x8597ce46,
+  0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, 0x82cc0f36, 0x825a0a5b,
+  0x81f1d1ce, 0x81936daf,
+  0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, 0x804fd23a, 0x802ce84c,
+  0x8013f61d, 0x8004fda0,
+
+};
+
+
+/**    
+ * @brief  Q31 sin_cos function.   
+ * @param[in]  theta    scaled input value in degrees    
+ * @param[out] *pSinVal points to the processed sine output.    
+ * @param[out] *pCosVal points to the processed cosine output.    
+ * @return none.   
+ *    
+ * The Q31 input value is in the range [-1 0.999999] and is mapped to a degree value in the range [-180 179].   
+ *    
+ */
+
+
+void arm_sin_cos_q31(
+  q31_t theta,
+  q31_t * pSinVal,
+  q31_t * pCosVal)
+{
+  q31_t x0;                                      /* Nearest input value */
+  q31_t y0, y1;                                  /* Nearest output values */
+  q31_t xSpacing = INPUT_SPACING;                /* Spaing between inputs */
+  int32_t i;                                     /* Index */
+  q31_t oneByXSpacing;                           /* 1/ xSpacing value */
+  q31_t out;                                     /* temporary variable */
+  uint32_t sign_bits;                            /* No.of sign bits */
+  uint32_t firstX = 0x80000000;                  /* First X value */
+
+  /* Calculation of index */
+  i = ((uint32_t) theta - firstX) / (uint32_t) xSpacing;
+
+  /* Checking min and max index of table */
+  if(i < 0)
+  {
+    i = 0;
+  }
+  else if(i >= 359)
+  {
+    i = 358;
+  }
+
+  /* Calculation of first nearest input value */
+  x0 = (q31_t) firstX + ((q31_t) i * xSpacing);
+
+  /* Reading nearest sine output values from table */
+  y0 = sinTableQ31[i];
+  y1 = sinTableQ31[i + 1u];
+
+  /* Calculation of 1/(x1-x0) */
+  /* (x1-x0) is xSpacing which is fixed value */
+  sign_bits = 8u;
+  oneByXSpacing = 0x5A000000;
+
+  /* Calculation of (theta - x0)/(x1-x0) */
+  out =
+    (((q31_t) (((q63_t) (theta - x0) * oneByXSpacing) >> 32)) << sign_bits);
+
+  /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
+  *pSinVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
+
+  /* Reading nearest cosine output values from table */
+  y0 = cosTableQ31[i];
+  y1 = cosTableQ31[i + 1u];
+
+  /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
+  *pCosVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
+
+}
+
+/**    
+ * @} end of SinCos group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c

@@ -0,0 +1,280 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cos_f32.c    
+*    
+* Description:	Fast cosine calculation for floating-point values.   
+*    
+* 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 groupFastMath    
+ */
+
+/**    
+ * @defgroup cos Cosine    
+ *    
+ * Computes the trigonometric cosine function using a combination of table lookup   
+ * and cubic interpolation.  There are separate functions for   
+ * Q15, Q31, and floating-point data types.   
+ * The input to the floating-point version is in radians while the   
+ * fixed-point Q15 and Q31 have a scaled input with the range   
+ * [0 +0.9999] mapping to [0 2*pi), Where range excludes 2*pi.   
+ *   
+ * The implementation is based on table lookup using 256 values together with cubic interpolation.   
+ * The steps used are:   
+ *  -# Calculation of the nearest integer table index   
+ *  -# Fetch the four table values a, b, c, and d     
+ *  -# Compute the fractional portion (fract) of the table index.   
+ *  -# Calculation of wa, wb, wc, wd    
+ *  -# The final result equals <code>a*wa + b*wb + c*wc + d*wd</code>   
+ *   
+ * where   
+ * <pre>    
+ *    a=Table[index-1];    
+ *    b=Table[index+0];    
+ *    c=Table[index+1];    
+ *    d=Table[index+2];    
+ * </pre>   
+ * and   
+ * <pre>    
+ *    wa=-(1/6)*fract.^3 + (1/2)*fract.^2 - (1/3)*fract;    
+ *    wb=(1/2)*fract.^3 - fract.^2 - (1/2)*fract + 1;    
+ *    wc=-(1/2)*fract.^3+(1/2)*fract.^2+fract;    
+ *    wd=(1/6)*fract.^3 - (1/6)*fract;    
+ * </pre>    
+ */
+
+ /**    
+ * @addtogroup cos    
+ * @{    
+ */
+
+
+/**    
+* \par    
+* <b>Example code for Generation of Cos Table:</b>   
+* tableSize = 256;    
+* <pre>for(n = -1; n < (tableSize + 2); n++)    
+* {    
+*	cosTable[n+1]= cos(2*pi*n/tableSize);    
+* } </pre>    
+* where pi value is  3.14159265358979    
+*/
+
+static const float32_t cosTable[260] = {
+  0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
+  0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
+  0.992479562759399410f, 0.989176511764526370f,
+  0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
+  0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
+  0.949528157711029050f, 0.941544055938720700f,
+  0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
+  0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
+  0.870086967945098880f, 0.857728600502014160f,
+  0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
+  0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
+  0.757208824157714840f, 0.740951120853424070f,
+  0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
+  0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
+  0.615231573581695560f, 0.595699310302734380f,
+  0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
+  0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
+  0.449611335992813110f, 0.427555084228515630f,
+  0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
+  0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
+  0.266712754964828490f, 0.242980182170867920f,
+  0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
+  0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
+  0.073564566671848297f, 0.049067676067352295f,
+  0.024541229009628296f, 0.000000000000000061f, -0.024541229009628296f,
+  -0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
+  -0.122410677373409270f, -0.146730467677116390f,
+  -0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
+  -0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
+  -0.313681751489639280f, -0.336889863014221190f,
+  -0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
+  -0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
+  -0.492898195981979370f, -0.514102756977081300f,
+  -0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
+  -0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
+  -0.653172850608825680f, -0.671558976173400880f,
+  -0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
+  -0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
+  -0.788346409797668460f, -0.803207516670227050f,
+  -0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
+  -0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
+  -0.893224298954010010f, -0.903989315032958980f,
+  -0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
+  -0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
+  -0.963776051998138430f, -0.970031261444091800f,
+  -0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
+  -0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
+  -0.997290432453155520f, -0.998795449733734130f,
+  -0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
+  -0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
+  -0.992479562759399410f, -0.989176511764526370f,
+  -0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
+  -0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
+  -0.949528157711029050f, -0.941544055938720700f,
+  -0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
+  -0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
+  -0.870086967945098880f, -0.857728600502014160f,
+  -0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
+  -0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
+  -0.757208824157714840f, -0.740951120853424070f,
+  -0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
+  -0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
+  -0.615231573581695560f, -0.595699310302734380f,
+  -0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
+  -0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
+  -0.449611335992813110f, -0.427555084228515630f,
+  -0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
+  -0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
+  -0.266712754964828490f, -0.242980182170867920f,
+  -0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
+  -0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
+  -0.073564566671848297f, -0.049067676067352295f,
+  -0.024541229009628296f, -0.000000000000000184f, 0.024541229009628296f,
+  0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
+  0.122410677373409270f, 0.146730467677116390f,
+  0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
+  0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
+  0.313681751489639280f, 0.336889863014221190f,
+  0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
+  0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
+  0.492898195981979370f, 0.514102756977081300f,
+  0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
+  0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
+  0.653172850608825680f, 0.671558976173400880f,
+  0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
+  0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
+  0.788346409797668460f, 0.803207516670227050f,
+  0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
+  0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
+  0.893224298954010010f, 0.903989315032958980f,
+  0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
+  0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
+  0.963776051998138430f, 0.970031261444091800f,
+  0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
+  0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
+  0.997290432453155520f, 0.998795449733734130f,
+  0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
+  0.998795449733734130f
+};
+
+/**   
+ * @brief  Fast approximation to the trigonometric cosine function for floating-point data.   
+ * @param[in] x input value in radians.   
+ * @return cos(x).   
+ */
+
+
+float32_t arm_cos_f32(
+  float32_t x)
+{
+  float32_t cosVal, fract, in;
+  int32_t index;
+  uint32_t tableSize = (uint32_t) TABLE_SIZE;
+  float32_t wa, wb, wc, wd;
+  float32_t a, b, c, d;
+  float32_t *tablePtr;
+  int32_t n;
+  float32_t fractsq, fractby2, fractby6, fractby3, fractsqby2;
+  float32_t oneminusfractby2;
+  float32_t frby2xfrsq, frby6xfrsq;
+
+  /* input x is in radians */
+  /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
+  in = x * 0.159154943092f;
+
+  /* Calculation of floor value of input */
+  n = (int32_t) in;
+
+  /* Make negative values towards -infinity */
+  if(x < 0.0f)
+  {
+    n = n - 1;
+  }
+
+  /* Map input value to [0 1] */
+  in = in - (float32_t) n;
+
+  /* Calculation of index of the table */
+  index = (uint32_t) (tableSize * in);
+
+  /* fractional value calculation */
+  fract = ((float32_t) tableSize * in) - (float32_t) index;
+
+  /* Checking min and max index of table */
+  if(index < 0)
+  {
+    index = 0;
+  }
+  else if(index > 256)
+  {
+    index = 256;
+  }
+
+  /* Initialise table pointer */
+  tablePtr = (float32_t *) & cosTable[index];
+
+  /* Read four nearest values of input value from the cos table */
+  a = tablePtr[0];
+  b = tablePtr[1];
+  c = tablePtr[2];
+  d = tablePtr[3];
+
+  /* Cubic interpolation process */
+  fractsq = fract * fract;
+  fractby2 = fract * 0.5f;
+  fractby6 = fract * 0.166666667f;
+  fractby3 = fract * 0.3333333333333f;
+  fractsqby2 = fractsq * 0.5f;
+  frby2xfrsq = (fractby2) * fractsq;
+  frby6xfrsq = (fractby6) * fractsq;
+  oneminusfractby2 = 1.0f - fractby2;
+  wb = fractsqby2 - fractby3;
+  wc = (fractsqby2 + fract);
+  wa = wb - frby6xfrsq;
+  wb = frby2xfrsq - fractsq;
+  cosVal = wa * a;
+  wc = wc - frby2xfrsq;
+  wd = (frby6xfrsq) - fractby6;
+  wb = wb + oneminusfractby2;
+
+  /* Calculate cos value */
+  cosVal = (cosVal + (b * wb)) + ((c * wc) + (d * wd));
+
+  /* Return the output value */
+  return (cosVal);
+
+}
+
+/**    
+ * @} end of cos group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q15.c

@@ -0,0 +1,205 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cos_q15.c    
+*    
+* Description:	Fast cosine calculation for Q15 values.   
+*    
+* 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 groupFastMath    
+ */
+
+ /**    
+ * @addtogroup cos    
+ * @{    
+ */
+
+/**   
+* \par    
+* Table Values are in Q15(1.15 Fixed point format) and generation is done in three steps    
+* \par    
+* First Generate cos values in floating point:    
+* tableSize = 256;     
+* <pre>for(n = -1; n < (tableSize + 1); n++)    
+* {    
+*	cosTable[n+1]= cos(2*pi*n/tableSize);    
+* }</pre>   
+* where pi value is  3.14159265358979    
+* \par    
+* Secondly Convert Floating point to Q15(Fixed point):    
+*	(cosTable[i] * pow(2, 15))    
+* \par    
+* Finally Rounding to nearest integer is done    
+* 	cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);    
+*/
+
+static const q15_t cosTableQ15[259] = {
+  0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d,
+  0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885,
+  0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca,
+  0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7,
+  0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40,
+  0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba,
+  0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a,
+  0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648,
+  0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38,
+  0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1,
+  0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32,
+  0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a,
+  0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930,
+  0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a,
+  0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6,
+  0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027,
+  0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163,
+  0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b,
+  0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236,
+  0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129,
+  0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0,
+  0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946,
+  0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6,
+  0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8,
+  0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8,
+  0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f,
+  0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce,
+  0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6,
+  0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0,
+  0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6,
+  0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a,
+  0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9,
+  0x7ff6, 0x7fff, 0x7ff6
+};
+
+
+/**   
+ * @brief Fast approximation to the trigonometric cosine function for Q15 data.   
+ * @param[in] x Scaled input value in radians.   
+ * @return  cos(x).   
+ *   
+ * The Q15 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.   
+ */
+
+q15_t arm_cos_q15(
+  q15_t x)
+{
+  q31_t cosVal;                                  /* Temporary variable for output */
+  q15_t *tablePtr;                               /* Pointer to table */
+  q15_t in, in2;                                 /* Temporary variables for input */
+  q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */
+  q15_t a, b, c, d;                              /* Four nearest output values */
+  q15_t fract, fractCube, fractSquare;           /* Variables for fractional value */
+  q15_t oneBy6 = 0x1555;                         /* Fixed point value of 1/6 */
+  q15_t tableSpacing = TABLE_SPACING_Q15;        /* Table spacing */
+  int32_t index;                                 /* Index variable */
+
+  in = x;
+
+  /* Calculate the nearest index */
+  index = (int32_t) in / tableSpacing;
+
+  /* Calculate the nearest value of input */
+  in2 = (q15_t) index *tableSpacing;
+
+  /* Calculation of fractional value */
+  fract = (in - in2) << 8;
+
+  /* fractSquare = fract * fract */
+  fractSquare = (q15_t) ((fract * fract) >> 15);
+
+  /* fractCube = fract * fract * fract */
+  fractCube = (q15_t) ((fractSquare * fract) >> 15);
+
+  /* Checking min and max index of table */
+  if(index < 0)
+  {
+    index = 0;
+  }
+  else if(index > 256)
+  {
+    index = 256;
+  }
+
+  /* Initialise table pointer */
+  tablePtr = (q15_t *) & cosTableQ15[index];
+
+  /* Cubic interpolation process */
+  /* Calculation of wa */
+  /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAA)*fract; */
+  wa = (q31_t) oneBy6 *fractCube;
+  wa += (q31_t) 0x2AAA *fract;
+  wa = -(wa >> 15);
+  wa += (fractSquare >> 1u);
+
+  /* Read first nearest value of output from the cos table */
+  a = *tablePtr++;
+
+  /* cosVal = a * wa */
+  cosVal = a * wa;
+
+  /* Calculation of wb */
+  wb = (((fractCube >> 1u) - fractSquare) - (fract >> 1u)) + 0x7FFF;
+
+  /* Read second nearest value of output from the cos table */
+  b = *tablePtr++;
+
+  /*      cosVal += b*wb */
+  cosVal += b * wb;
+
+  /* Calculation of wc */
+  wc = -(q31_t) fractCube + fractSquare;
+  wc = (wc >> 1u) + fract;
+
+  /* Read third nearest value of output from the cos table */
+  c = *tablePtr++;
+
+  /* cosVal += c*wc */
+  cosVal += c * wc;
+
+  /* Calculation of wd */
+  /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
+  fractCube = fractCube - fract;
+  wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15));
+
+  /* Read fourth nearest value of output from the cos table */
+  d = *tablePtr++;
+
+  /* cosVal += d*wd; */
+  cosVal += d * wd;
+
+  /* Convert output value in 1.15(q15) format and saturate */
+  cosVal = __SSAT((cosVal >> 15), 16);
+
+  /* Return the output value in 1.15(q15) format */
+  return ((q15_t) cosVal);
+
+}
+
+/**    
+ * @} end of cos group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q31.c

@@ -0,0 +1,239 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_cos_q31.c    
+*    
+* Description:	Fast cosine calculation for Q31 values.   
+*    
+* 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 groupFastMath    
+ */
+
+ /**    
+ * @addtogroup cos    
+ * @{    
+ */
+
+/**    
+ * \par    
+ * Table Values are in Q31(1.31 Fixed point format) and generation is done in three steps    
+ * First Generate cos values in floating point:    
+ * tableSize = 256;      
+ * <pre>for(n = -1; n < (tableSize + 1); n++)    
+ * {    
+ *	cosTable[n+1]= cos(2*pi*n/tableSize);    
+ * } </pre>     
+ * where pi value is  3.14159265358979    
+ * \par    
+ * Secondly Convert Floating point to Q31(Fixed point):    
+ *	(cosTable[i] * pow(2, 31))    
+ * \par    
+ * Finally Rounding to nearest integer is done    
+ * 	cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);    
+ */
+
+
+static const q31_t cosTableQ31[259] = {
+  0x7ff62182, 0x7fffffff, 0x7ff62182, 0x7fd8878e, 0x7fa736b4, 0x7f62368f,
+  0x7f0991c4, 0x7e9d55fc,
+  0x7e1d93ea, 0x7d8a5f40, 0x7ce3ceb2, 0x7c29fbee, 0x7b5d039e, 0x7a7d055b,
+  0x798a23b1, 0x78848414,
+  0x776c4edb, 0x7641af3d, 0x7504d345, 0x73b5ebd1, 0x72552c85, 0x70e2cbc6,
+  0x6f5f02b2, 0x6dca0d14,
+  0x6c242960, 0x6a6d98a4, 0x68a69e81, 0x66cf8120, 0x64e88926, 0x62f201ac,
+  0x60ec3830, 0x5ed77c8a,
+  0x5cb420e0, 0x5a82799a, 0x5842dd54, 0x55f5a4d2, 0x539b2af0, 0x5133cc94,
+  0x4ebfe8a5, 0x4c3fdff4,
+  0x49b41533, 0x471cece7, 0x447acd50, 0x41ce1e65, 0x3f1749b8, 0x3c56ba70,
+  0x398cdd32, 0x36ba2014,
+  0x33def287, 0x30fbc54d, 0x2e110a62, 0x2b1f34eb, 0x2826b928, 0x25280c5e,
+  0x2223a4c5, 0x1f19f97b,
+  0x1c0b826a, 0x18f8b83c, 0x15e21445, 0x12c8106f, 0xfab272b, 0xc8bd35e,
+  0x96a9049, 0x647d97c,
+  0x3242abf, 0x0, 0xfcdbd541, 0xf9b82684, 0xf6956fb7, 0xf3742ca2, 0xf054d8d5,
+  0xed37ef91,
+  0xea1debbb, 0xe70747c4, 0xe3f47d96, 0xe0e60685, 0xdddc5b3b, 0xdad7f3a2,
+  0xd7d946d8, 0xd4e0cb15,
+  0xd1eef59e, 0xcf043ab3, 0xcc210d79, 0xc945dfec, 0xc67322ce, 0xc3a94590,
+  0xc0e8b648, 0xbe31e19b,
+  0xbb8532b0, 0xb8e31319, 0xb64beacd, 0xb3c0200c, 0xb140175b, 0xaecc336c,
+  0xac64d510, 0xaa0a5b2e,
+  0xa7bd22ac, 0xa57d8666, 0xa34bdf20, 0xa1288376, 0x9f13c7d0, 0x9d0dfe54,
+  0x9b1776da, 0x99307ee0,
+  0x9759617f, 0x9592675c, 0x93dbd6a0, 0x9235f2ec, 0x90a0fd4e, 0x8f1d343a,
+  0x8daad37b, 0x8c4a142f,
+  0x8afb2cbb, 0x89be50c3, 0x8893b125, 0x877b7bec, 0x8675dc4f, 0x8582faa5,
+  0x84a2fc62, 0x83d60412,
+  0x831c314e, 0x8275a0c0, 0x81e26c16, 0x8162aa04, 0x80f66e3c, 0x809dc971,
+  0x8058c94c, 0x80277872,
+  0x8009de7e, 0x80000000, 0x8009de7e, 0x80277872, 0x8058c94c, 0x809dc971,
+  0x80f66e3c, 0x8162aa04,
+  0x81e26c16, 0x8275a0c0, 0x831c314e, 0x83d60412, 0x84a2fc62, 0x8582faa5,
+  0x8675dc4f, 0x877b7bec,
+  0x8893b125, 0x89be50c3, 0x8afb2cbb, 0x8c4a142f, 0x8daad37b, 0x8f1d343a,
+  0x90a0fd4e, 0x9235f2ec,
+  0x93dbd6a0, 0x9592675c, 0x9759617f, 0x99307ee0, 0x9b1776da, 0x9d0dfe54,
+  0x9f13c7d0, 0xa1288376,
+  0xa34bdf20, 0xa57d8666, 0xa7bd22ac, 0xaa0a5b2e, 0xac64d510, 0xaecc336c,
+  0xb140175b, 0xb3c0200c,
+  0xb64beacd, 0xb8e31319, 0xbb8532b0, 0xbe31e19b, 0xc0e8b648, 0xc3a94590,
+  0xc67322ce, 0xc945dfec,
+  0xcc210d79, 0xcf043ab3, 0xd1eef59e, 0xd4e0cb15, 0xd7d946d8, 0xdad7f3a2,
+  0xdddc5b3b, 0xe0e60685,
+  0xe3f47d96, 0xe70747c4, 0xea1debbb, 0xed37ef91, 0xf054d8d5, 0xf3742ca2,
+  0xf6956fb7, 0xf9b82684,
+  0xfcdbd541, 0x0, 0x3242abf, 0x647d97c, 0x96a9049, 0xc8bd35e, 0xfab272b,
+  0x12c8106f,
+  0x15e21445, 0x18f8b83c, 0x1c0b826a, 0x1f19f97b, 0x2223a4c5, 0x25280c5e,
+  0x2826b928, 0x2b1f34eb,
+  0x2e110a62, 0x30fbc54d, 0x33def287, 0x36ba2014, 0x398cdd32, 0x3c56ba70,
+  0x3f1749b8, 0x41ce1e65,
+  0x447acd50, 0x471cece7, 0x49b41533, 0x4c3fdff4, 0x4ebfe8a5, 0x5133cc94,
+  0x539b2af0, 0x55f5a4d2,
+  0x5842dd54, 0x5a82799a, 0x5cb420e0, 0x5ed77c8a, 0x60ec3830, 0x62f201ac,
+  0x64e88926, 0x66cf8120,
+  0x68a69e81, 0x6a6d98a4, 0x6c242960, 0x6dca0d14, 0x6f5f02b2, 0x70e2cbc6,
+  0x72552c85, 0x73b5ebd1,
+  0x7504d345, 0x7641af3d, 0x776c4edb, 0x78848414, 0x798a23b1, 0x7a7d055b,
+  0x7b5d039e, 0x7c29fbee,
+  0x7ce3ceb2, 0x7d8a5f40, 0x7e1d93ea, 0x7e9d55fc, 0x7f0991c4, 0x7f62368f,
+  0x7fa736b4, 0x7fd8878e,
+  0x7ff62182, 0x7fffffff, 0x7ff62182
+};
+
+/**   
+ * @brief Fast approximation to the trigonometric cosine function for Q31 data.   
+ * @param[in] x Scaled input value in radians.   
+ * @return  cos(x).   
+ *   
+ * The Q31 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.   
+ */
+
+q31_t arm_cos_q31(
+  q31_t x)
+{
+  q31_t cosVal, in, in2;                         /* Temporary variables for input, output */
+  q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */
+  q31_t a, b, c, d;                              /* Four nearest output values */
+  q31_t *tablePtr;                               /* Pointer to table */
+  q31_t fract, fractCube, fractSquare;           /* Temporary values for fractional values */
+  q31_t oneBy6 = 0x15555555;                     /* Fixed point value of 1/6 */
+  q31_t tableSpacing = TABLE_SPACING_Q31;        /* Table spacing */
+  q31_t temp;                                    /* Temporary variable for intermediate process */
+  int32_t index;                                 /* Index variable */
+
+  in = x;
+
+  /* Calculate the nearest index */
+  index = in / tableSpacing;
+
+  /* Calculate the nearest value of input */
+  in2 = ((q31_t) index) * tableSpacing;
+
+  /* Calculation of fractional value */
+  fract = (in - in2) << 8;
+
+  /* fractSquare = fract * fract */
+  fractSquare = ((q31_t) (((q63_t) fract * fract) >> 32));
+  fractSquare = fractSquare << 1;
+
+  /* fractCube = fract * fract * fract */
+  fractCube = ((q31_t) (((q63_t) fractSquare * fract) >> 32));
+  fractCube = fractCube << 1;
+
+  /* Checking min and max index of table */
+  if(index < 0)
+  {
+    index = 0;
+  }
+  else if(index > 256)
+  {
+    index = 256;
+  }
+
+  /* Initialise table pointer */
+  tablePtr = (q31_t *) & cosTableQ31[index];
+
+  /* Cubic interpolation process */
+  /* Calculation of wa */
+  /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAAAAAA)*fract; */
+  wa = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
+  temp = 0x2AAAAAAA;
+  wa = (q31_t) ((((q63_t) wa << 32) + ((q63_t) temp * fract)) >> 32);
+  wa = -(wa << 1u);
+  wa += (fractSquare >> 1u);
+
+  /* Read first nearest value of output from the cos table */
+  a = *tablePtr++;
+
+  /* cosVal = a*wa */
+  cosVal = ((q31_t) (((q63_t) a * wa) >> 32));
+
+  /* q31(1.31) Fixed point value of 1 */
+  temp = 0x7FFFFFFF;
+
+  /* Calculation of wb */
+  wb = ((fractCube >> 1u) - (fractSquare + (fract >> 1u))) + temp;
+  /* Read second nearest value of output from the cos table */
+  b = *tablePtr++;
+
+  /*      cosVal += b*wb */
+  cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) b * (wb))) >> 32);
+
+  /* Calculation of wc */
+  wc = -fractCube + fractSquare;
+  wc = (wc >> 1u) + fract;
+  /* Read third nearest values of output value from the cos table */
+  c = *tablePtr++;
+
+  /*      cosVal += c*wc */
+  cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) c * (wc))) >> 32);
+
+  /* Calculation of wd */
+  /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
+  fractCube = fractCube - fract;
+  wd = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
+  wd = (wd << 1u);
+
+  /* Read fourth nearest value of output from the cos table */
+  d = *tablePtr++;
+
+  /* cosVal += d*wd; */
+  cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) d * (wd))) >> 32);
+
+
+  /* convert cosVal in 2.30 format to 1.31 format */
+  return (__QADD(cosVal, cosVal));
+
+}
+
+/**    
+ * @} end of cos group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c

@@ -0,0 +1,281 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sin_f32.c    
+*    
+* Description:	Fast sine calculation for floating-point values.   
+*    
+* 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 groupFastMath    
+ */
+
+/**    
+ * @defgroup sin Sine    
+ *    
+ * Computes the trigonometric sine function using a combination of table lookup   
+ * and cubic interpolation.  There are separate functions for   
+ * Q15, Q31, and floating-point data types.   
+ * The input to the floating-point version is in radians while the   
+ * fixed-point Q15 and Q31 have a scaled input with the range   
+ * [0 +0.9999] mapping to [0 2*pi), Where range excludes 2*pi.   
+ *   
+ * The implementation is based on table lookup using 256 values together with cubic interpolation.   
+ * The steps used are:   
+ *  -# Calculation of the nearest integer table index   
+ *  -# Fetch the four table values a, b, c, and d     
+ *  -# Compute the fractional portion (fract) of the table index.   
+ *  -# Calculation of wa, wb, wc, wd    
+ *  -# The final result equals <code>a*wa + b*wb + c*wc + d*wd</code>   
+ *   
+ * where   
+ * <pre>    
+ *    a=Table[index-1];    
+ *    b=Table[index+0];    
+ *    c=Table[index+1];    
+ *    d=Table[index+2];    
+ * </pre>   
+ * and   
+ * <pre>    
+ *    wa=-(1/6)*fract.^3 + (1/2)*fract.^2 - (1/3)*fract;    
+ *    wb=(1/2)*fract.^3 - fract.^2 - (1/2)*fract + 1;    
+ *    wc=-(1/2)*fract.^3+(1/2)*fract.^2+fract;    
+ *    wd=(1/6)*fract.^3 - (1/6)*fract;    
+ * </pre>    
+ */
+
+/**    
+ * @addtogroup sin    
+ * @{    
+ */
+
+
+/**   
+ * \par    
+ * Example code for Generation of Floating-point Sin Table:   
+ * tableSize = 256;    
+ * <pre>for(n = -1; n < (tableSize + 1); n++)    
+ * {    
+ *	sinTable[n+1]=sin(2*pi*n/tableSize);    
+ * }</pre>    
+ * \par    
+ * where pi value is  3.14159265358979    
+ */
+
+static const float32_t sinTable[259] = {
+  -0.024541229009628296f, 0.000000000000000000f, 0.024541229009628296f,
+  0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
+  0.122410677373409270f, 0.146730467677116390f,
+  0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
+  0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
+  0.313681751489639280f, 0.336889863014221190f,
+  0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
+  0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
+  0.492898195981979370f, 0.514102756977081300f,
+  0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
+  0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
+  0.653172850608825680f, 0.671558976173400880f,
+  0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
+  0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
+  0.788346409797668460f, 0.803207516670227050f,
+  0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
+  0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
+  0.893224298954010010f, 0.903989315032958980f,
+  0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
+  0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
+  0.963776051998138430f, 0.970031261444091800f,
+  0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
+  0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
+  0.997290432453155520f, 0.998795449733734130f,
+  0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
+  0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
+  0.992479562759399410f, 0.989176511764526370f,
+  0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
+  0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
+  0.949528157711029050f, 0.941544055938720700f,
+  0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
+  0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
+  0.870086967945098880f, 0.857728600502014160f,
+  0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
+  0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
+  0.757208824157714840f, 0.740951120853424070f,
+  0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
+  0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
+  0.615231573581695560f, 0.595699310302734380f,
+  0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
+  0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
+  0.449611335992813110f, 0.427555084228515630f,
+  0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
+  0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
+  0.266712754964828490f, 0.242980182170867920f,
+  0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
+  0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
+  0.073564566671848297f, 0.049067676067352295f,
+  0.024541229009628296f, 0.000000000000000122f, -0.024541229009628296f,
+  -0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
+  -0.122410677373409270f, -0.146730467677116390f,
+  -0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
+  -0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
+  -0.313681751489639280f, -0.336889863014221190f,
+  -0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
+  -0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
+  -0.492898195981979370f, -0.514102756977081300f,
+  -0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
+  -0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
+  -0.653172850608825680f, -0.671558976173400880f,
+  -0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
+  -0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
+  -0.788346409797668460f, -0.803207516670227050f,
+  -0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
+  -0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
+  -0.893224298954010010f, -0.903989315032958980f,
+  -0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
+  -0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
+  -0.963776051998138430f, -0.970031261444091800f,
+  -0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
+  -0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
+  -0.997290432453155520f, -0.998795449733734130f,
+  -0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
+  -0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
+  -0.992479562759399410f, -0.989176511764526370f,
+  -0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
+  -0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
+  -0.949528157711029050f, -0.941544055938720700f,
+  -0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
+  -0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
+  -0.870086967945098880f, -0.857728600502014160f,
+  -0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
+  -0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
+  -0.757208824157714840f, -0.740951120853424070f,
+  -0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
+  -0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
+  -0.615231573581695560f, -0.595699310302734380f,
+  -0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
+  -0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
+  -0.449611335992813110f, -0.427555084228515630f,
+  -0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
+  -0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
+  -0.266712754964828490f, -0.242980182170867920f,
+  -0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
+  -0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
+  -0.073564566671848297f, -0.049067676067352295f,
+  -0.024541229009628296f, -0.000000000000000245f, 0.024541229009628296f
+};
+
+
+/**   
+ * @brief  Fast approximation to the trigonometric sine function for floating-point data.   
+ * @param[in] x input value in radians.   
+ * @return  sin(x).   
+ */
+
+float32_t arm_sin_f32(
+  float32_t x)
+{
+  float32_t sinVal, fract, in;                   /* Temporary variables for input, output */
+  int32_t index;                                 /* Index variable */
+  uint32_t tableSize = (uint32_t) TABLE_SIZE;    /* Initialise tablesize */
+  float32_t wa, wb, wc, wd;                      /* Cubic interpolation coefficients */
+  float32_t a, b, c, d;                          /* Four nearest output values */
+  float32_t *tablePtr;                           /* Pointer to table */
+  int32_t n;
+  float32_t fractsq, fractby2, fractby6, fractby3, fractsqby2;
+  float32_t oneminusfractby2;
+  float32_t frby2xfrsq, frby6xfrsq;
+
+  /* input x is in radians */
+  /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
+  in = x * 0.159154943092f;
+
+  /* Calculation of floor value of input */
+  n = (int32_t) in;
+
+  /* Make negative values towards -infinity */
+  if(x < 0.0f)
+  {
+    n = n - 1;
+  }
+
+  /* Map input value to [0 1] */
+  in = in - (float32_t) n;
+
+  /* Calculation of index of the table */
+  index = (uint32_t) (tableSize * in);
+
+  /* fractional value calculation */
+  fract = ((float32_t) tableSize * in) - (float32_t) index;
+
+  /* Checking min and max index of table */
+  if(index < 0)
+  {
+    index = 0;
+  }
+  else if(index > 256)
+  {
+    index = 256;
+  }
+
+  /* Initialise table pointer */
+  tablePtr = (float32_t *) & sinTable[index];
+
+  /* Read four nearest values of input value from the sin table */
+  a = tablePtr[0];
+  b = tablePtr[1];
+  c = tablePtr[2];
+  d = tablePtr[3];
+
+  /* Cubic interpolation process */
+  fractsq = fract * fract;
+  fractby2 = fract * 0.5f;
+  fractby6 = fract * 0.166666667f;
+  fractby3 = fract * 0.3333333333333f;
+  fractsqby2 = fractsq * 0.5f;
+  frby2xfrsq = (fractby2) * fractsq;
+  frby6xfrsq = (fractby6) * fractsq;
+  oneminusfractby2 = 1.0f - fractby2;
+  wb = fractsqby2 - fractby3;
+  wc = (fractsqby2 + fract);
+  wa = wb - frby6xfrsq;
+  wb = frby2xfrsq - fractsq;
+  sinVal = wa * a;
+  wc = wc - frby2xfrsq;
+  wd = (frby6xfrsq) - fractby6;
+  wb = wb + oneminusfractby2;
+
+  /* Calculate sin value */
+  sinVal = (sinVal + (b * wb)) + ((c * wc) + (d * wd));
+
+  /* Return the output value */
+  return (sinVal);
+
+}
+
+/**    
+ * @} end of sin group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q15.c

@@ -0,0 +1,208 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sin_q15.c    
+*    
+* Description:	Fast sine calculation for Q15 values.   
+*    
+* 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 groupFastMath    
+ */
+
+ /**    
+ * @addtogroup sin    
+ * @{    
+ */
+
+
+/**    
+ * \par    
+ * Example code for Generation of Q15 Sin Table:   
+ * \par    
+ * <pre>tableSize = 256;    
+ * for(n = -1; n < (tableSize + 1); n++)    
+ * {    
+ *	sinTable[n+1]=sin(2*pi*n/tableSize);    
+ * } </pre>    
+ * where pi value is  3.14159265358979    
+ * \par    
+ * Convert Floating point to Q15(Fixed point):    
+ *	(sinTable[i] * pow(2, 15))    
+ * \par    
+ * rounding to nearest integer is done    
+ * 	sinTable[i] += (sinTable[i] > 0 ? 0.5 :-0.5);    
+ */
+
+
+static const q15_t sinTableQ15[259] = {
+  0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8,
+  0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f,
+  0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce,
+  0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6,
+  0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0,
+  0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6,
+  0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a,
+  0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9,
+  0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d,
+  0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885,
+  0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca,
+  0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7,
+  0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40,
+  0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba,
+  0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a,
+  0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648,
+  0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38,
+  0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1,
+  0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32,
+  0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a,
+  0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930,
+  0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a,
+  0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6,
+  0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027,
+  0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163,
+  0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b,
+  0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236,
+  0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129,
+  0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0,
+  0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946,
+  0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6,
+  0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8,
+  0xfcdc, 0x0, 0x324
+};
+
+
+/**   
+ * @brief Fast approximation to the trigonometric sine function for Q15 data.   
+ * @param[in] x Scaled input value in radians.   
+ * @return  sin(x).   
+ *   
+ * The Q15 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.   
+ */
+
+q15_t arm_sin_q15(
+  q15_t x)
+{
+  q31_t sinVal;                                  /* Temporary variables output */
+  q15_t *tablePtr;                               /* Pointer to table */
+  q15_t fract, in, in2;                          /* Temporary variables for input, output */
+  q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */
+  q15_t a, b, c, d;                              /* Four nearest output values */
+  q15_t fractCube, fractSquare;                  /* Temporary values for fractional value */
+  q15_t oneBy6 = 0x1555;                         /* Fixed point value of 1/6 */
+  q15_t tableSpacing = TABLE_SPACING_Q15;        /* Table spacing */
+  int32_t index;                                 /* Index variable */
+
+  in = x;
+
+  /* Calculate the nearest index */
+  index = (int32_t) in / tableSpacing;
+
+  /* Calculate the nearest value of input */
+  in2 = (q15_t) ((index) * tableSpacing);
+
+  /* Calculation of fractional value */
+  fract = (in - in2) << 8;
+
+  /* fractSquare = fract * fract */
+  fractSquare = (q15_t) ((fract * fract) >> 15);
+
+  /* fractCube = fract * fract * fract */
+  fractCube = (q15_t) ((fractSquare * fract) >> 15);
+
+  /* Checking min and max index of table */
+  if(index < 0)
+  {
+    index = 0;
+  }
+  else if(index > 256)
+  {
+    index = 256;
+  }
+
+  /* Initialise table pointer */
+  tablePtr = (q15_t *) & sinTableQ15[index];
+
+  /* Cubic interpolation process */
+  /* Calculation of wa */
+  /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAA)*fract; */
+  wa = (q31_t) oneBy6 *fractCube;
+  wa += (q31_t) 0x2AAA *fract;
+  wa = -(wa >> 15);
+  wa += ((q31_t) fractSquare >> 1u);
+
+  /* Read first nearest value of output from the sin table */
+  a = *tablePtr++;
+
+  /* sinVal = a * wa */
+  sinVal = a * wa;
+
+  /* Calculation of wb */
+  wb = (((q31_t) fractCube >> 1u) - (q31_t) fractSquare) -
+    (((q31_t) fract >> 1u) - 0x7FFF);
+
+  /* Read second nearest value of output from the sin table */
+  b = *tablePtr++;
+
+  /*      sinVal += b*wb */
+  sinVal += b * wb;
+
+
+  /* Calculation of wc */
+  wc = -(q31_t) fractCube + fractSquare;
+  wc = (wc >> 1u) + fract;
+
+  /* Read third nearest value of output from the sin table */
+  c = *tablePtr++;
+
+  /* sinVal += c*wc */
+  sinVal += c * wc;
+
+  /* Calculation of wd */
+  /* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
+  fractCube = fractCube - fract;
+  wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15));
+
+  /* Read fourth nearest value of output from the sin table */
+  d = *tablePtr++;
+
+  /* sinVal += d*wd; */
+  sinVal += d * wd;
+
+  /* Convert output value in 1.15(q15) format and saturate */
+  sinVal = __SSAT((sinVal >> 15), 16);
+
+  /* Return the output value in 1.15(q15) format */
+  return ((q15_t) sinVal);
+
+}
+
+/**    
+ * @} end of sin group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q31.c

@@ -0,0 +1,240 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:		arm_sin_q31.c    
+*    
+* Description:	Fast sine calculation for Q31 values.   
+*    
+* 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 groupFastMath    
+ */
+
+ /**    
+ * @addtogroup sin    
+ * @{    
+ */
+
+/**   
+ * \par    
+ * Tables generated are in Q31(1.31 Fixed point format)    
+ * Generation of sin values in floating point:    
+ * <pre>tableSize = 256;      
+ * for(n = -1; n < (tableSize + 1); n++)    
+ * {    
+ *	sinTable[n+1]= sin(2*pi*n/tableSize);    
+ * } </pre>    
+ * where pi value is  3.14159265358979    
+ * \par    
+ * Convert Floating point to Q31(Fixed point):    
+ *	(sinTable[i] * pow(2, 31))    
+ * \par    
+ * rounding to nearest integer is done    
+ * 	sinTable[i] += (sinTable[i] > 0 ? 0.5 :-0.5);    
+ */
+
+static const q31_t sinTableQ31[259] = {
+  0xfcdbd541, 0x0, 0x3242abf, 0x647d97c, 0x96a9049, 0xc8bd35e, 0xfab272b,
+  0x12c8106f,
+  0x15e21445, 0x18f8b83c, 0x1c0b826a, 0x1f19f97b, 0x2223a4c5, 0x25280c5e,
+  0x2826b928, 0x2b1f34eb,
+  0x2e110a62, 0x30fbc54d, 0x33def287, 0x36ba2014, 0x398cdd32, 0x3c56ba70,
+  0x3f1749b8, 0x41ce1e65,
+  0x447acd50, 0x471cece7, 0x49b41533, 0x4c3fdff4, 0x4ebfe8a5, 0x5133cc94,
+  0x539b2af0, 0x55f5a4d2,
+  0x5842dd54, 0x5a82799a, 0x5cb420e0, 0x5ed77c8a, 0x60ec3830, 0x62f201ac,
+  0x64e88926, 0x66cf8120,
+  0x68a69e81, 0x6a6d98a4, 0x6c242960, 0x6dca0d14, 0x6f5f02b2, 0x70e2cbc6,
+  0x72552c85, 0x73b5ebd1,
+  0x7504d345, 0x7641af3d, 0x776c4edb, 0x78848414, 0x798a23b1, 0x7a7d055b,
+  0x7b5d039e, 0x7c29fbee,
+  0x7ce3ceb2, 0x7d8a5f40, 0x7e1d93ea, 0x7e9d55fc, 0x7f0991c4, 0x7f62368f,
+  0x7fa736b4, 0x7fd8878e,
+  0x7ff62182, 0x7fffffff, 0x7ff62182, 0x7fd8878e, 0x7fa736b4, 0x7f62368f,
+  0x7f0991c4, 0x7e9d55fc,
+  0x7e1d93ea, 0x7d8a5f40, 0x7ce3ceb2, 0x7c29fbee, 0x7b5d039e, 0x7a7d055b,
+  0x798a23b1, 0x78848414,
+  0x776c4edb, 0x7641af3d, 0x7504d345, 0x73b5ebd1, 0x72552c85, 0x70e2cbc6,
+  0x6f5f02b2, 0x6dca0d14,
+  0x6c242960, 0x6a6d98a4, 0x68a69e81, 0x66cf8120, 0x64e88926, 0x62f201ac,
+  0x60ec3830, 0x5ed77c8a,
+  0x5cb420e0, 0x5a82799a, 0x5842dd54, 0x55f5a4d2, 0x539b2af0, 0x5133cc94,
+  0x4ebfe8a5, 0x4c3fdff4,
+  0x49b41533, 0x471cece7, 0x447acd50, 0x41ce1e65, 0x3f1749b8, 0x3c56ba70,
+  0x398cdd32, 0x36ba2014,
+  0x33def287, 0x30fbc54d, 0x2e110a62, 0x2b1f34eb, 0x2826b928, 0x25280c5e,
+  0x2223a4c5, 0x1f19f97b,
+  0x1c0b826a, 0x18f8b83c, 0x15e21445, 0x12c8106f, 0xfab272b, 0xc8bd35e,
+  0x96a9049, 0x647d97c,
+  0x3242abf, 0x0, 0xfcdbd541, 0xf9b82684, 0xf6956fb7, 0xf3742ca2, 0xf054d8d5,
+  0xed37ef91,
+  0xea1debbb, 0xe70747c4, 0xe3f47d96, 0xe0e60685, 0xdddc5b3b, 0xdad7f3a2,
+  0xd7d946d8, 0xd4e0cb15,
+  0xd1eef59e, 0xcf043ab3, 0xcc210d79, 0xc945dfec, 0xc67322ce, 0xc3a94590,
+  0xc0e8b648, 0xbe31e19b,
+  0xbb8532b0, 0xb8e31319, 0xb64beacd, 0xb3c0200c, 0xb140175b, 0xaecc336c,
+  0xac64d510, 0xaa0a5b2e,
+  0xa7bd22ac, 0xa57d8666, 0xa34bdf20, 0xa1288376, 0x9f13c7d0, 0x9d0dfe54,
+  0x9b1776da, 0x99307ee0,
+  0x9759617f, 0x9592675c, 0x93dbd6a0, 0x9235f2ec, 0x90a0fd4e, 0x8f1d343a,
+  0x8daad37b, 0x8c4a142f,
+  0x8afb2cbb, 0x89be50c3, 0x8893b125, 0x877b7bec, 0x8675dc4f, 0x8582faa5,
+  0x84a2fc62, 0x83d60412,
+  0x831c314e, 0x8275a0c0, 0x81e26c16, 0x8162aa04, 0x80f66e3c, 0x809dc971,
+  0x8058c94c, 0x80277872,
+  0x8009de7e, 0x80000000, 0x8009de7e, 0x80277872, 0x8058c94c, 0x809dc971,
+  0x80f66e3c, 0x8162aa04,
+  0x81e26c16, 0x8275a0c0, 0x831c314e, 0x83d60412, 0x84a2fc62, 0x8582faa5,
+  0x8675dc4f, 0x877b7bec,
+  0x8893b125, 0x89be50c3, 0x8afb2cbb, 0x8c4a142f, 0x8daad37b, 0x8f1d343a,
+  0x90a0fd4e, 0x9235f2ec,
+  0x93dbd6a0, 0x9592675c, 0x9759617f, 0x99307ee0, 0x9b1776da, 0x9d0dfe54,
+  0x9f13c7d0, 0xa1288376,
+  0xa34bdf20, 0xa57d8666, 0xa7bd22ac, 0xaa0a5b2e, 0xac64d510, 0xaecc336c,
+  0xb140175b, 0xb3c0200c,
+  0xb64beacd, 0xb8e31319, 0xbb8532b0, 0xbe31e19b, 0xc0e8b648, 0xc3a94590,
+  0xc67322ce, 0xc945dfec,
+  0xcc210d79, 0xcf043ab3, 0xd1eef59e, 0xd4e0cb15, 0xd7d946d8, 0xdad7f3a2,
+  0xdddc5b3b, 0xe0e60685,
+  0xe3f47d96, 0xe70747c4, 0xea1debbb, 0xed37ef91, 0xf054d8d5, 0xf3742ca2,
+  0xf6956fb7, 0xf9b82684,
+  0xfcdbd541, 0x0, 0x3242abf
+};
+
+
+/**   
+ * @brief Fast approximation to the trigonometric sine function for Q31 data.   
+ * @param[in] x Scaled input value in radians.   
+ * @return  sin(x).   
+ *   
+ * The Q31 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.   
+ */
+
+q31_t arm_sin_q31(
+  q31_t x)
+{
+  q31_t sinVal, in, in2;                         /* Temporary variables for input, output */
+  int32_t index;                                 /* Index variables */
+  q31_t wa, wb, wc, wd;                          /* Cubic interpolation coefficients */
+  q31_t a, b, c, d;                              /* Four nearest output values */
+  q31_t *tablePtr;                               /* Pointer to table */
+  q31_t fract, fractCube, fractSquare;           /* Temporary values for fractional values */
+  q31_t oneBy6 = 0x15555555;                     /* Fixed point value of 1/6 */
+  q31_t tableSpacing = TABLE_SPACING_Q31;        /* Table spacing */
+  q31_t temp;                                    /* Temporary variable for intermediate process */
+
+  in = x;
+
+  /* Calculate the nearest index */
+  index = (uint32_t) in / (uint32_t) tableSpacing;
+
+  /* Calculate the nearest value of input */
+  in2 = (q31_t) index *tableSpacing;
+
+  /* Calculation of fractional value */
+  fract = (in - in2) << 8;
+
+  /* fractSquare = fract * fract */
+  fractSquare = ((q31_t) (((q63_t) fract * fract) >> 32));
+  fractSquare = fractSquare << 1;
+
+  /* fractCube = fract * fract * fract */
+  fractCube = ((q31_t) (((q63_t) fractSquare * fract) >> 32));
+  fractCube = fractCube << 1;
+
+  /* Checking min and max index of table */
+  if(index < 0)
+  {
+    index = 0;
+  }
+  else if(index > 256)
+  {
+    index = 256;
+  }
+
+  /* Initialise table pointer */
+  tablePtr = (q31_t *) & sinTableQ31[index];
+
+  /* Cubic interpolation process */
+  /* Calculation of wa */
+  /* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAAAAAA)*fract; */
+  wa = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
+  temp = 0x2AAAAAAA;
+  wa = (q31_t) ((((q63_t) wa << 32) + ((q63_t) temp * fract)) >> 32);
+  wa = -(wa << 1u);
+  wa += (fractSquare >> 1u);
+
+  /* Read first nearest value of output from the sin table */
+  a = *tablePtr++;
+
+  /* sinVal = a*wa */
+  sinVal = ((q31_t) (((q63_t) a * wa) >> 32));
+
+  /* q31(1.31) Fixed point value of 1 */
+  temp = 0x7FFFFFFF;
+
+  /* Calculation of wb */
+  wb = ((fractCube >> 1u) - (fractSquare + (fract >> 1u))) + temp;
+
+  /* Read second nearest value of output from the sin table */
+  b = *tablePtr++;
+
+  /*  sinVal += b*wb */
+  sinVal = (q31_t) ((((q63_t) sinVal << 32) + (q63_t) b * (wb)) >> 32);
+
+  /* Calculation of wc */
+  wc = -fractCube + fractSquare;
+  wc = (wc >> 1u) + fract;
+
+  /* Read third nearest value of output from the sin table */
+  c = *tablePtr++;
+
+  /*      sinVal += c*wc */
+  sinVal = (q31_t) ((((q63_t) sinVal << 32) + ((q63_t) c * wc)) >> 32);
+
+  /* Calculation of wd */
+  /* wd = (oneBy6) * fractCube - (oneBy6) * fract; */
+  fractCube = fractCube - fract;
+  wd = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
+  wd = (wd << 1u);
+
+  /* Read fourth nearest value of output from the sin table */
+  d = *tablePtr++;
+
+  /* sinVal += d*wd; */
+  sinVal = (q31_t) ((((q63_t) sinVal << 32) + ((q63_t) d * wd)) >> 32);
+
+  /* convert sinVal in 2.30 format to 1.31 format */
+  return (__QADD(sinVal, sinVal));
+
+}
+
+/**    
+ * @} end of sin group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c

@@ -0,0 +1,131 @@
+/* ----------------------------------------------------------------------     
+* Copyright (C) 2011 ARM Limited. All rights reserved.  
+*     
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*     
+* Project:      CMSIS DSP Library  
+* Title:		arm_sqrt_q15.c     
+*     
+* Description:	Q15 square root function.    
+*     
+* 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.0 2011/03/08 
+*     Alpha release. 
+* 
+* Version 1.0.1 2011/09/30 
+*     Beta release.  
+*     
+* -------------------------------------------------------------------- */
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+
+/**     
+ * @ingroup groupFastMath     
+ */
+
+/**     
+ * @addtogroup SQRT     
+ * @{     
+ */
+
+  /**    
+   * @brief  Q15 square root function.    
+   * @param[in]   in     input value.  The range of the input value is [0 +1) or 0x0000 to 0x7FFF.    
+   * @param[out]  *pOut  square root of input value.    
+   * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if    
+   * <code>in</code> is negative value and returns zero output for negative values.    
+   */
+
+arm_status arm_sqrt_q15(
+  q15_t in,
+  q15_t * pOut)
+{
+  q15_t number, temp1, var1, signBits1, half;
+  q31_t bits_val1;
+  float32_t temp_float1;
+
+  number = in;
+
+  /* If the input is a positive number then compute the signBits. */
+  if(number > 0)
+  {
+    signBits1 = __CLZ(number) - 17;
+
+    /* Shift by the number of signBits1 */
+    if((signBits1 % 2) == 0)
+    {
+      number = number << signBits1;
+    }
+    else
+    {
+      number = number << (signBits1 - 1);
+    }
+
+    /* Calculate half value of the number */
+    half = number >> 1;
+    /* Store the number for later use */
+    temp1 = number;
+
+    /*Convert to float */
+    temp_float1 = number * 3.051757812500000e-005f;
+    /*Store as integer */
+    bits_val1 = *(int *) &temp_float1;
+    /* Subtract the shifted value from the magic number to give intial guess */
+    bits_val1 = 0x5f3759df - (bits_val1 >> 1);  // gives initial guess  
+    /* Store as float */
+    temp_float1 = *(float *) &bits_val1;
+    /* Convert to integer format */
+    var1 = (q31_t) (temp_float1 * 16384);
+
+    /* 1st iteration */
+    var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
+                                     ((q15_t)
+                                      ((((q15_t)
+                                         (((q31_t) var1 * var1) >> 15)) *
+                                        (q31_t) half) >> 15))) >> 15)) << 2;
+    /* 2nd iteration */
+    var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
+                                     ((q15_t)
+                                      ((((q15_t)
+                                         (((q31_t) var1 * var1) >> 15)) *
+                                        (q31_t) half) >> 15))) >> 15)) << 2;
+    /* 3rd iteration */
+    var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
+                                     ((q15_t)
+                                      ((((q15_t)
+                                         (((q31_t) var1 * var1) >> 15)) *
+                                        (q31_t) half) >> 15))) >> 15)) << 2;
+
+    /* Multiply the inverse square root with the original value */
+    var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1;
+
+    /* Shift the output down accordingly */
+    if((signBits1 % 2) == 0)
+    {
+      var1 = var1 >> (signBits1 / 2);
+    }
+    else
+    {
+      var1 = var1 >> ((signBits1 - 1) / 2);
+    }
+    *pOut = var1;
+
+    return (ARM_MATH_SUCCESS);
+  }
+  /* If the number is a negative number then store zero as its square root value */
+  else
+  {
+    *pOut = 0;
+    return (ARM_MATH_ARGUMENT_ERROR);
+  }
+}
+
+/**     
+ * @} end of SQRT group     
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q31.c

@@ -0,0 +1,129 @@
+/* ----------------------------------------------------------------------     
+* Copyright (C) 2011 ARM Limited. All rights reserved.  
+*     
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*     
+* Project:      CMSIS DSP Library  
+* Title:		arm_sqrt_q31.c     
+*     
+* Description:	Q31 square root function.    
+*     
+* 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.0 2011/03/08 
+*     Alpha release. 
+* 
+* Version 1.0.1 2011/09/30 
+*     Beta release.  
+*     
+* -------------------------------------------------------------------- */
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+/**     
+ * @ingroup groupFastMath     
+ */
+
+/**     
+ * @addtogroup SQRT     
+ * @{     
+ */
+
+/**    
+ * @brief Q31 square root function.    
+ * @param[in]   in    input value.  The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.    
+ * @param[out]  *pOut square root of input value.    
+ * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if    
+ * <code>in</code> is negative value and returns zero output for negative values.    
+ */
+
+arm_status arm_sqrt_q31(
+  q31_t in,
+  q31_t * pOut)
+{
+  q31_t number, temp1, bits_val1, var1, signBits1, half;
+  float32_t temp_float1;
+
+  number = in;
+
+  /* If the input is a positive number then compute the signBits. */
+  if(number > 0)
+  {
+    signBits1 = __CLZ(number) - 1;
+
+    /* Shift by the number of signBits1 */
+    if((signBits1 % 2) == 0)
+    {
+      number = number << signBits1;
+    }
+    else
+    {
+      number = number << (signBits1 - 1);
+    }
+
+    /* Calculate half value of the number */
+    half = number >> 1;
+    /* Store the number for later use */
+    temp1 = number;
+
+    /*Convert to float */
+    temp_float1 = number * 4.6566128731e-010f;
+    /*Store as integer */
+    bits_val1 = *(int *) &temp_float1;
+    /* Subtract the shifted value from the magic number to give intial guess */
+    bits_val1 = 0x5f3759df - (bits_val1 >> 1);  // gives initial guess  
+    /* Store as float */
+    temp_float1 = *(float *) &bits_val1;
+    /* Convert to integer format */
+    var1 = (q31_t) (temp_float1 * 1073741824);
+
+    /* 1st iteration */
+    var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
+                                     ((q31_t)
+                                      ((((q31_t)
+                                         (((q63_t) var1 * var1) >> 31)) *
+                                        (q63_t) half) >> 31))) >> 31)) << 2;
+    /* 2nd iteration */
+    var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
+                                     ((q31_t)
+                                      ((((q31_t)
+                                         (((q63_t) var1 * var1) >> 31)) *
+                                        (q63_t) half) >> 31))) >> 31)) << 2;
+    /* 3rd iteration */
+    var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
+                                     ((q31_t)
+                                      ((((q31_t)
+                                         (((q63_t) var1 * var1) >> 31)) *
+                                        (q63_t) half) >> 31))) >> 31)) << 2;
+
+    /* Multiply the inverse square root with the original value */
+    var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
+
+    /* Shift the output down accordingly */
+    if((signBits1 % 2) == 0)
+    {
+      var1 = var1 >> (signBits1 / 2);
+    }
+    else
+    {
+      var1 = var1 >> ((signBits1 - 1) / 2);
+    }
+    *pOut = var1;
+
+    return (ARM_MATH_SUCCESS);
+  }
+  /* If the number is a negative number then store zero as its square root value */
+  else
+  {
+    *pOut = 0;
+    return (ARM_MATH_ARGUMENT_ERROR);
+  }
+}
+
+/**     
+ * @} end of SQRT group     
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c

@@ -0,0 +1,105 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_biquad_cascade_df1_32x64_init_q31.c    
+*    
+* Description:	High precision Q31 Biquad cascade filter initialization function.    
+*    
+* 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 groupFilters    
+ */
+
+/**    
+ * @addtogroup BiquadCascadeDF1_32x64    
+ * @{    
+ */
+
+/**    
+ * @details    
+ *    
+ * @param[in,out] *S           	points to an instance of the high precision Q31 Biquad cascade filter structure.    
+ * @param[in]     numStages     number of 2nd order stages in the filter.    
+ * @param[in]     *pCoeffs      points to the filter coefficients.    
+ * @param[in]     *pState       points to the state buffer.    
+ * @param[in]     postShift     Shift to be applied after the accumulator.  Varies according to the coefficients format.    
+ * @return        none    
+ *    
+ * <b>Coefficient and State Ordering:</b>    
+ *    
+ * \par    
+ * The coefficients are stored in the array <code>pCoeffs</code> in the following order:    
+ * <pre>    
+ *     {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}    
+ * </pre>    
+ * where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,    
+ * <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,    
+ * and so on.  The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.    
+ *    
+ * \par    
+ * The <code>pState</code> points to state variables array and size of each state variable is 1.63 format.    
+ * Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.    
+ * The state variables are arranged in the state array as:    
+ * <pre>    
+ *     {x[n-1], x[n-2], y[n-1], y[n-2]}    
+ * </pre>    
+ * The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.    
+ * The state array has a total length of <code>4*numStages</code> values.    
+ * The state variables are updated after each block of data is processed; the coefficients are untouched.    
+ */
+
+void arm_biquad_cas_df1_32x64_init_q31(
+  arm_biquad_cas_df1_32x64_ins_q31 * S,
+  uint8_t numStages,
+  q31_t * pCoeffs,
+  q63_t * pState,
+  uint8_t postShift)
+{
+  /* Assign filter stages */
+  S->numStages = numStages;
+
+  /* Assign postShift to be applied to the output */
+  S->postShift = postShift;
+
+  /* Assign coefficient pointer */
+  S->pCoeffs = pCoeffs;
+
+  /* Clear state buffer and size is always 4 * numStages */
+  memset(pState, 0, (4u * (uint32_t) numStages) * sizeof(q63_t));
+
+  /* Assign state pointer */
+  S->pState = pState;
+}
+
+/**    
+ * @} end of BiquadCascadeDF1_32x64 group    
+ */

apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c

@@ -0,0 +1,553 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_biquad_cascade_df1_32x64_q31.c    
+*    
+* Description:	High precision Q31 Biquad cascade filter processing function    
+*    
+* 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 groupFilters    
+ */
+
+/**    
+ * @defgroup BiquadCascadeDF1_32x64 High Precision Q31 Biquad Cascade Filter    
+ *    
+ * This function implements a high precision Biquad cascade filter which operates on    
+ * Q31 data values.  The filter coefficients are in 1.31 format and the state variables    
+ * are in 1.63 format.  The double precision state variables reduce quantization noise    
+ * in the filter and provide a cleaner output.    
+ * These filters are particularly useful when implementing filters in which the    
+ * singularities are close to the unit circle.  This is common for low pass or high    
+ * pass filters with very low cutoff frequencies.    
+ *    
+ * The function operates on blocks of input and output data    
+ * and each call to the function processes <code>blockSize</code> samples through    
+ * the filter. <code>pSrc</code> and <code>pDst</code> points to input and output arrays    
+ * containing <code>blockSize</code> Q31 values.    
+ *    
+ * \par Algorithm    
+ * Each Biquad stage implements a second order filter using the difference equation:    
+ * <pre>    
+ *     y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]    
+ * </pre>    
+ * A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage.    
+ * \image html Biquad.gif "Single Biquad filter stage"    
+ * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.    
+ * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.    
+ * Pay careful attention to the sign of the feedback coefficients.    
+ * Some design tools use the difference equation    
+ * <pre>    
+ *     y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]    
+ * </pre>    
+ * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.    
+ *    
+ * \par    
+ * Higher order filters are realized as a cascade of second order sections.    
+ * <code>numStages</code> refers to the number of second order stages used.    
+ * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.    
+ * \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages"    
+ * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).    
+ *    
+ * \par    
+ * The <code>pState</code> points to state variables array .    
+ * Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code> and each state variable in 1.63 format to improve precision.    
+ * The state variables are arranged in the array as:    
+ * <pre>    
+ *     {x[n-1], x[n-2], y[n-1], y[n-2]}    
+ * </pre>    
+ *    
+ * \par    
+ * The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.    
+ * The state array has a total length of <code>4*numStages</code> values of data in 1.63 format.    
+ * The state variables are updated after each block of data is processed; the coefficients are untouched.    
+ *    
+ * \par Instance Structure    
+ * The coefficients and state variables for a filter are stored together in an instance data structure.    
+ * A separate instance structure must be defined for each filter.    
+ * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.    
+ *    
+ * \par Init Function    
+ * There is also an associated initialization function which performs the following operations:    
+ * - Sets the values of the internal structure fields.    
+ * - Zeros out the values in the state buffer.    
+ * \par    
+ * Use of the initialization function is optional.    
+ * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.    
+ * To place an instance structure into a const data section, the instance structure must be manually initialized.    
+ * Set the values in the state buffer to zeros before static initialization.    
+ * For example, to statically initialize the filter instance structure use    
+ * <pre>    
+ *     arm_biquad_cas_df1_32x64_ins_q31 S1 = {numStages, pState, pCoeffs, postShift};    
+ * </pre>    
+ * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer;    
+ * <code>pCoeffs</code> is the address of the coefficient buffer; <code>postShift</code> shift to be applied which is described in detail below.    
+ * \par Fixed-Point Behavior    
+ * Care must be taken while using Biquad Cascade 32x64 filter function.    
+ * Following issues must be considered:    
+ * - Scaling of coefficients    
+ * - Filter gain    
+ * - Overflow and saturation    
+ *    
+ * \par    
+ * Filter coefficients are represented as fractional values and    
+ * restricted to lie in the range <code>[-1 +1)</code>.    
+ * The processing function has an additional scaling parameter <code>postShift</code>    
+ * which allows the filter coefficients to exceed the range <code>[+1 -1)</code>.    
+ * At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.    
+ * \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator"    
+ * This essentially scales the filter coefficients by <code>2^postShift</code>.    
+ * For example, to realize the coefficients    
+ * <pre>    
+ *    {1.5, -0.8, 1.2, 1.6, -0.9}    
+ * </pre>    
+ * set the Coefficient array to:    
+ * <pre>    
+ *    {0.75, -0.4, 0.6, 0.8, -0.45}    
+ * </pre>    
+ * and set <code>postShift=1</code>    
+ *    
+ * \par    
+ * The second thing to keep in mind is the gain through the filter.    
+ * The frequency response of a Biquad filter is a function of its coefficients.    
+ * It is possible for the gain through the filter to exceed 1.0 meaning that the filter increases the amplitude of certain frequencies.    
+ * This means that an input signal with amplitude < 1.0 may result in an output > 1.0 and these are saturated or overflowed based on the implementation of the filter.    
+ * To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0 or the input signal must be scaled down so that the combination of input and filter are never overflowed.    
+ *    
+ * \par    
+ * The third item to consider is the overflow and saturation behavior of the fixed-point Q31 version.    
+ * This is described in the function specific documentation below.    
+ */
+
+/**    
+ * @addtogroup BiquadCascadeDF1_32x64    
+ * @{    
+ */
+
+/**    
+ * @details    
+    
+ * @param[in]  *S points to an instance of the high precision Q31 Biquad cascade filter.    
+ * @param[in]  *pSrc points to the block of input data.    
+ * @param[out] *pDst points to the block of output data.    
+ * @param[in]  blockSize number of samples to process.    
+ * @return none.    
+ *    
+ * \par    
+ * The function is implemented using an internal 64-bit accumulator.    
+ * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.    
+ * Thus, if the accumulator result overflows it wraps around rather than clip.    
+ * In order to avoid overflows completely the input signal must be scaled down by 2 bits and lie in the range [-0.25 +0.25).    
+ * After all 5 multiply-accumulates are performed, the 2.62 accumulator is shifted by <code>postShift</code> bits and the result truncated to    
+ * 1.31 format by discarding the low 32 bits.    
+ *    
+ * \par    
+ * Two related functions are provided in the CMSIS DSP library.    
+ * <code>arm_biquad_cascade_df1_q31()</code> implements a Biquad cascade with 32-bit coefficients and state variables with a Q63 accumulator.    
+ * <code>arm_biquad_cascade_df1_fast_q31()</code> implements a Biquad cascade with 32-bit coefficients and state variables with a Q31 accumulator.    
+ */
+
+void arm_biquad_cas_df1_32x64_q31(
+  const arm_biquad_cas_df1_32x64_ins_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize)
+{
+  q31_t *pIn = pSrc;                             /*  input pointer initialization  */
+  q31_t *pOut = pDst;                            /*  output pointer initialization */
+  q63_t *pState = S->pState;                     /*  state pointer initialization  */
+  q31_t *pCoeffs = S->pCoeffs;                   /*  coeff pointer initialization  */
+  q63_t acc;                                     /*  accumulator                   */
+  q31_t Xn1, Xn2;                                /*  Input Filter state variables        */
+  q63_t Yn1, Yn2;                                /*  Output Filter state variables        */
+  q31_t b0, b1, b2, a1, a2;                      /*  Filter coefficients           */
+  q31_t Xn;                                      /*  temporary input               */
+  int32_t shift = (int32_t) S->postShift + 1;    /*  Shift to be applied to the output */
+  uint32_t sample, stage = S->numStages;         /*  loop counters                     */
+  q31_t acc_l, acc_h;                            /*  temporary output               */
+  uint32_t uShift = ((uint32_t) S->postShift + 1u);
+  uint32_t lShift = 32u - uShift;                /*  Shift to be applied to the output */
+
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  do
+  {
+    /* Reading the coefficients */
+    b0 = *pCoeffs++;
+    b1 = *pCoeffs++;
+    b2 = *pCoeffs++;
+    a1 = *pCoeffs++;
+    a2 = *pCoeffs++;
+
+    /* Reading the state values */
+    Xn1 = (q31_t) (pState[0]);
+    Xn2 = (q31_t) (pState[1]);
+    Yn1 = pState[2];
+    Yn2 = pState[3];
+
+    /* Apply loop unrolling and compute 4 output values simultaneously. */
+    /* The variable acc hold output value that is being computed and    
+     * stored in the destination buffer    
+     * acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]    
+     */
+
+    sample = 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(sample > 0u)
+    {
+      /* Read the input */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+      /* acc =  b0 * x[n] */
+      acc = (q63_t) Xn *b0;
+
+      /* acc +=  b1 * x[n-1] */
+      acc += (q63_t) Xn1 *b1;
+
+      /* acc +=  b[2] * x[n-2] */
+      acc += (q63_t) Xn2 *b2;
+
+      /* acc +=  a1 * y[n-1] */
+      acc += mult32x64(Yn1, a1);
+
+      /* acc +=  a2 * y[n-2] */
+      acc += mult32x64(Yn2, a2);
+
+      /* The result is converted to 1.63 , Yn2 variable is reused */
+      Yn2 = acc << shift;
+
+      /* Calc lower part of acc */
+      acc_l = acc & 0xffffffff;
+
+      /* Calc upper part of acc */
+      acc_h = (acc >> 32) & 0xffffffff;
+
+      /* Apply shift for lower part of acc and upper part of acc */
+      acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+      /* Store the output in the destination buffer in 1.31 format. */
+      *pOut = acc_h;
+
+      /* Read the second input into Xn2, to reuse the value */
+      Xn2 = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+      /* acc +=  b1 * x[n-1] */
+      acc = (q63_t) Xn *b1;
+
+      /* acc =  b0 * x[n] */
+      acc += (q63_t) Xn2 *b0;
+
+      /* acc +=  b[2] * x[n-2] */
+      acc += (q63_t) Xn1 *b2;
+
+      /* acc +=  a1 * y[n-1] */
+      acc += mult32x64(Yn2, a1);
+
+      /* acc +=  a2 * y[n-2] */
+      acc += mult32x64(Yn1, a2);
+
+      /* The result is converted to 1.63, Yn1 variable is reused */
+      Yn1 = acc << shift;
+
+      /* Calc lower part of acc */
+      acc_l = acc & 0xffffffff;
+
+      /* Calc upper part of acc */
+      acc_h = (acc >> 32) & 0xffffffff;
+
+      /* Apply shift for lower part of acc and upper part of acc */
+      acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+      /* Read the third input into Xn1, to reuse the value */
+      Xn1 = *pIn++;
+
+      /* The result is converted to 1.31 */
+      /* Store the output in the destination buffer. */
+      *(pOut + 1u) = acc_h;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+      /* acc =  b0 * x[n] */
+      acc = (q63_t) Xn1 *b0;
+
+      /* acc +=  b1 * x[n-1] */
+      acc += (q63_t) Xn2 *b1;
+
+      /* acc +=  b[2] * x[n-2] */
+      acc += (q63_t) Xn *b2;
+
+      /* acc +=  a1 * y[n-1] */
+      acc += mult32x64(Yn1, a1);
+
+      /* acc +=  a2 * y[n-2] */
+      acc += mult32x64(Yn2, a2);
+
+      /* The result is converted to 1.63, Yn2 variable is reused  */
+      Yn2 = acc << shift;
+
+      /* Calc lower part of acc */
+      acc_l = acc & 0xffffffff;
+
+      /* Calc upper part of acc */
+      acc_h = (acc >> 32) & 0xffffffff;
+
+      /* Apply shift for lower part of acc and upper part of acc */
+      acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+      /* Store the output in the destination buffer in 1.31 format. */
+      *(pOut + 2u) = acc_h;
+
+      /* Read the fourth input into Xn, to reuse the value */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      /* acc =  b0 * x[n] */
+      acc = (q63_t) Xn *b0;
+
+      /* acc +=  b1 * x[n-1] */
+      acc += (q63_t) Xn1 *b1;
+
+      /* acc +=  b[2] * x[n-2] */
+      acc += (q63_t) Xn2 *b2;
+
+      /* acc +=  a1 * y[n-1] */
+      acc += mult32x64(Yn2, a1);
+
+      /* acc +=  a2 * y[n-2] */
+      acc += mult32x64(Yn1, a2);
+
+      /* The result is converted to 1.63, Yn1 variable is reused  */
+      Yn1 = acc << shift;
+
+      /* Calc lower part of acc */
+      acc_l = acc & 0xffffffff;
+
+      /* Calc upper part of acc */
+      acc_h = (acc >> 32) & 0xffffffff;
+
+      /* Apply shift for lower part of acc and upper part of acc */
+      acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+      /* Store the output in the destination buffer in 1.31 format. */
+      *(pOut + 3u) = acc_h;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc    */
+      Xn2 = Xn1;
+      Xn1 = Xn;
+
+      /* update output pointer */
+      pOut += 4u;
+
+      /* decrement the loop counter */
+      sample--;
+    }
+
+    /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+     ** No loop unrolling is used. */
+    sample = (blockSize & 0x3u);
+
+    while(sample > 0u)
+    {
+      /* Read the input */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+      /* acc =  b0 * x[n] */
+      acc = (q63_t) Xn *b0;
+      /* acc +=  b1 * x[n-1] */
+      acc += (q63_t) Xn1 *b1;
+      /* acc +=  b[2] * x[n-2] */
+      acc += (q63_t) Xn2 *b2;
+      /* acc +=  a1 * y[n-1] */
+      acc += mult32x64(Yn1, a1);
+      /* acc +=  a2 * y[n-2] */
+      acc += mult32x64(Yn2, a2);
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc    */
+      Xn2 = Xn1;
+      Xn1 = Xn;
+      Yn2 = Yn1;
+      /* The result is converted to 1.63, Yn1 variable is reused  */
+      Yn1 = acc << shift;
+
+      /* Calc lower part of acc */
+      acc_l = acc & 0xffffffff;
+
+      /* Calc upper part of acc */
+      acc_h = (acc >> 32) & 0xffffffff;
+
+      /* Apply shift for lower part of acc and upper part of acc */
+      acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+      /* Store the output in the destination buffer in 1.31 format. */
+      *pOut++ = acc_h;
+      //Yn1 = acc << shift; 
+
+      /* Store the output in the destination buffer in 1.31 format. */
+//      *pOut++ = (q31_t) (acc >> (32 - shift)); 
+
+      /* decrement the loop counter */
+      sample--;
+    }
+
+    /*  The first stage output is given as input to the second stage. */
+    pIn = pDst;
+
+    /* Reset to destination buffer working pointer */
+    pOut = pDst;
+
+    /*  Store the updated state variables back into the pState array */
+    /*  Store the updated state variables back into the pState array */
+    *pState++ = (q63_t) Xn1;
+    *pState++ = (q63_t) Xn2;
+    *pState++ = Yn1;
+    *pState++ = Yn2;
+
+  } while(--stage);
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  do
+  {
+    /* Reading the coefficients */
+    b0 = *pCoeffs++;
+    b1 = *pCoeffs++;
+    b2 = *pCoeffs++;
+    a1 = *pCoeffs++;
+    a2 = *pCoeffs++;
+
+    /* Reading the state values */
+    Xn1 = pState[0];
+    Xn2 = pState[1];
+    Yn1 = pState[2];
+    Yn2 = pState[3];
+
+    /* The variable acc hold output value that is being computed and        
+     * stored in the destination buffer            
+     * acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]            
+     */
+
+    sample = blockSize;
+
+    while(sample > 0u)
+    {
+      /* Read the input */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      /* acc =  b0 * x[n] */
+      acc = (q63_t) Xn *b0;
+      /* acc +=  b1 * x[n-1] */
+      acc += (q63_t) Xn1 *b1;
+      /* acc +=  b[2] * x[n-2] */
+      acc += (q63_t) Xn2 *b2;
+      /* acc +=  a1 * y[n-1] */
+      acc += mult32x64(Yn1, a1);
+      /* acc +=  a2 * y[n-2] */
+      acc += mult32x64(Yn2, a2);
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc    */
+      Xn2 = Xn1;
+      Xn1 = Xn;
+      Yn2 = Yn1;
+
+      /* The result is converted to 1.63, Yn1 variable is reused  */
+      Yn1 = acc << shift;
+
+      /* Calc lower part of acc */
+      acc_l = acc & 0xffffffff;
+
+      /* Calc upper part of acc */
+      acc_h = (acc >> 32) & 0xffffffff;
+
+      /* Apply shift for lower part of acc and upper part of acc */
+      acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+      /* Store the output in the destination buffer in 1.31 format. */
+      *pOut++ = acc_h;
+
+      //Yn1 = acc << shift; 
+
+      /* Store the output in the destination buffer in 1.31 format. */
+      //*pOut++ = (q31_t) (acc >> (32 - shift)); 
+
+      /* decrement the loop counter */
+      sample--;
+    }
+
+    /*  The first stage output is given as input to the second stage. */
+    pIn = pDst;
+
+    /* Reset to destination buffer working pointer */
+    pOut = pDst;
+
+    /*  Store the updated state variables back into the pState array */
+    *pState++ = (q63_t) Xn1;
+    *pState++ = (q63_t) Xn2;
+    *pState++ = Yn1;
+    *pState++ = Yn2;
+
+  } while(--stage);
+
+#endif /*    #ifndef ARM_MATH_CM0     */
+}
+
+  /**    
+   * @} end of BiquadCascadeDF1_32x64 group    
+   */

apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c

@@ -0,0 +1,421 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_biquad_cascade_df1_f32.c    
+*    
+* Description:	Processing function for the    
+*               floating-point Biquad cascade DirectFormI(DF1) filter.    
+*    
+* 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 groupFilters    
+ */
+
+/**    
+ * @defgroup BiquadCascadeDF1 Biquad Cascade IIR Filters Using Direct Form I Structure    
+ *    
+ * This set of functions implements arbitrary order recursive (IIR) filters.    
+ * The filters are implemented as a cascade of second order Biquad sections.    
+ * The functions support Q15, Q31 and floating-point data types.  
+ * Fast version of Q15 and Q31 also supported on CortexM4 and Cortex-M3.    
+ *    
+ * The functions operate on blocks of input and output data and each call to the function    
+ * processes <code>blockSize</code> samples through the filter.    
+ * <code>pSrc</code> points to the array of input data and    
+ * <code>pDst</code> points to the array of output data.    
+ * Both arrays contain <code>blockSize</code> values.    
+ *    
+ * \par Algorithm    
+ * Each Biquad stage implements a second order filter using the difference equation:    
+ * <pre>    
+ *     y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]    
+ * </pre>    
+ * A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage.    
+ * \image html Biquad.gif "Single Biquad filter stage"    
+ * Coefficients <code>b0, b1 and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.    
+ * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.    
+ * Pay careful attention to the sign of the feedback coefficients.    
+ * Some design tools use the difference equation    
+ * <pre>    
+ *     y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]    
+ * </pre>    
+ * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.    
+ *    
+ * \par    
+ * Higher order filters are realized as a cascade of second order sections.    
+ * <code>numStages</code> refers to the number of second order stages used.    
+ * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.    
+ * \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages"    
+ * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).    
+ *    
+ * \par    
+ * The <code>pState</code> points to state variables array.    
+ * Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.    
+ * The state variables are arranged in the <code>pState</code> array as:    
+ * <pre>    
+ *     {x[n-1], x[n-2], y[n-1], y[n-2]}    
+ * </pre>    
+ *    
+ * \par    
+ * The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.    
+ * The state array has a total length of <code>4*numStages</code> values.    
+ * The state variables are updated after each block of data is processed, the coefficients are untouched.    
+ *    
+ * \par Instance Structure    
+ * The coefficients and state variables for a filter are stored together in an instance data structure.    
+ * A separate instance structure must be defined for each filter.    
+ * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.    
+ * There are separate instance structure declarations for each of the 3 supported data types.    
+ *    
+ * \par Init Functions    
+ * There is also an associated initialization function for each data type.    
+ * The initialization function performs following operations:    
+ * - Sets the values of the internal structure fields.    
+ * - Zeros out the values in the state buffer.    
+ *    
+ * \par    
+ * Use of the initialization function is optional.    
+ * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.    
+ * To place an instance structure into a const data section, the instance structure must be manually initialized.    
+ * Set the values in the state buffer to zeros before static initialization.    
+ * The code below statically initializes each of the 3 different data type filter instance structures    
+ * <pre>    
+ *     arm_biquad_casd_df1_inst_f32 S1 = {numStages, pState, pCoeffs};    
+ *     arm_biquad_casd_df1_inst_q15 S2 = {numStages, pState, pCoeffs, postShift};    
+ *     arm_biquad_casd_df1_inst_q31 S3 = {numStages, pState, pCoeffs, postShift};    
+ * </pre>    
+ * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer;    
+ * <code>pCoeffs</code> is the address of the coefficient buffer; <code>postShift</code> shift to be applied.    
+ *    
+ * \par Fixed-Point Behavior    
+ * Care must be taken when using the Q15 and Q31 versions of the Biquad Cascade filter functions.    
+ * Following issues must be considered:    
+ * - Scaling of coefficients    
+ * - Filter gain    
+ * - Overflow and saturation    
+ *    
+ * \par    
+ * <b>Scaling of coefficients: </b>    
+ * Filter coefficients are represented as fractional values and    
+ * coefficients are restricted to lie in the range <code>[-1 +1)</code>.    
+ * The fixed-point functions have an additional scaling parameter <code>postShift</code>    
+ * which allow the filter coefficients to exceed the range <code>[+1 -1)</code>.    
+ * At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.    
+ * \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator"    
+ * This essentially scales the filter coefficients by <code>2^postShift</code>.    
+ * For example, to realize the coefficients    
+ * <pre>    
+ *    {1.5, -0.8, 1.2, 1.6, -0.9}    
+ * </pre>    
+ * set the pCoeffs array to:    
+ * <pre>    
+ *    {0.75, -0.4, 0.6, 0.8, -0.45}    
+ * </pre>    
+ * and set <code>postShift=1</code>    
+ *    
+ * \par    
+ * <b>Filter gain: </b>    
+ * The frequency response of a Biquad filter is a function of its coefficients.    
+ * It is possible for the gain through the filter to exceed 1.0 meaning that the filter increases the amplitude of certain frequencies.    
+ * This means that an input signal with amplitude < 1.0 may result in an output > 1.0 and these are saturated or overflowed based on the implementation of the filter.    
+ * To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0 or the input signal must be scaled down so that the combination of input and filter are never overflowed.    
+ *    
+ * \par    
+ * <b>Overflow and saturation: </b>    
+ * For Q15 and Q31 versions, it is described separately as part of the function specific documentation below.    
+ */
+
+/**    
+ * @addtogroup BiquadCascadeDF1    
+ * @{    
+ */
+
+/**    
+ * @param[in]  *S         points to an instance of the floating-point Biquad cascade structure.    
+ * @param[in]  *pSrc      points to the block of input data.    
+ * @param[out] *pDst      points to the block of output data.    
+ * @param[in]  blockSize  number of samples to process per call.    
+ * @return     none.    
+ *    
+ */
+
+void arm_biquad_cascade_df1_f32(
+  const arm_biquad_casd_df1_inst_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize)
+{
+  float32_t *pIn = pSrc;                         /*  source pointer            */
+  float32_t *pOut = pDst;                        /*  destination pointer       */
+  float32_t *pState = S->pState;                 /*  pState pointer            */
+  float32_t *pCoeffs = S->pCoeffs;               /*  coefficient pointer       */
+  float32_t acc;                                 /*  Simulates the accumulator */
+  float32_t b0, b1, b2, a1, a2;                  /*  Filter coefficients       */
+  float32_t Xn1, Xn2, Yn1, Yn2;                  /*  Filter pState variables   */
+  float32_t Xn;                                  /*  temporary input           */
+  uint32_t sample, stage = S->numStages;         /*  loop counters             */
+
+
+#ifndef ARM_MATH_CM0
+
+  /* Run the below code for Cortex-M4 and Cortex-M3 */
+
+  do
+  {
+    /* Reading the coefficients */
+    b0 = *pCoeffs++;
+    b1 = *pCoeffs++;
+    b2 = *pCoeffs++;
+    a1 = *pCoeffs++;
+    a2 = *pCoeffs++;
+
+    /* Reading the pState values */
+    Xn1 = pState[0];
+    Xn2 = pState[1];
+    Yn1 = pState[2];
+    Yn2 = pState[3];
+
+    /* Apply loop unrolling and compute 4 output values simultaneously. */
+    /*      The variable acc hold output values that are being computed:    
+     *    
+     *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1]   + a2 * y[n-2]    
+     *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1]   + a2 * y[n-2]    
+     *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1]   + a2 * y[n-2]    
+     *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1]   + a2 * y[n-2]    
+     */
+
+    sample = 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(sample > 0u)
+    {
+      /* Read the first input */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      Yn2 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
+
+      /* Store the result in the accumulator in the destination buffer. */
+      *pOut++ = Yn2;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+
+      /* Read the second input */
+      Xn2 = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      Yn1 = (b0 * Xn2) + (b1 * Xn) + (b2 * Xn1) + (a1 * Yn2) + (a2 * Yn1);
+
+      /* Store the result in the accumulator in the destination buffer. */
+      *pOut++ = Yn1;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+
+      /* Read the third input */
+      Xn1 = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      Yn2 = (b0 * Xn1) + (b1 * Xn2) + (b2 * Xn) + (a1 * Yn1) + (a2 * Yn2);
+
+      /* Store the result in the accumulator in the destination buffer. */
+      *pOut++ = Yn2;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as: */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+
+      /* Read the forth input */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      Yn1 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn2) + (a2 * Yn1);
+
+      /* Store the result in the accumulator in the destination buffer. */
+      *pOut++ = Yn1;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+      Xn2 = Xn1;
+      Xn1 = Xn;
+
+      /* decrement the loop counter */
+      sample--;
+
+    }
+
+    /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
+     ** No loop unrolling is used. */
+    sample = blockSize & 0x3u;
+
+    while(sample > 0u)
+    {
+      /* Read the input */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
+
+      /* Store the result in the accumulator in the destination buffer. */
+      *pOut++ = acc;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:    */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+      Xn2 = Xn1;
+      Xn1 = Xn;
+      Yn2 = Yn1;
+      Yn1 = acc;
+
+      /* decrement the loop counter */
+      sample--;
+
+    }
+
+    /*  Store the updated state variables back into the pState array */
+    *pState++ = Xn1;
+    *pState++ = Xn2;
+    *pState++ = Yn1;
+    *pState++ = Yn2;
+
+    /*  The first stage goes from the input buffer to the output buffer. */
+    /*  Subsequent numStages  occur in-place in the output buffer */
+    pIn = pDst;
+
+    /* Reset the output pointer */
+    pOut = pDst;
+
+    /* decrement the loop counter */
+    stage--;
+
+  } while(stage > 0u);
+
+#else
+
+  /* Run the below code for Cortex-M0 */
+
+  do
+  {
+    /* Reading the coefficients */
+    b0 = *pCoeffs++;
+    b1 = *pCoeffs++;
+    b2 = *pCoeffs++;
+    a1 = *pCoeffs++;
+    a2 = *pCoeffs++;
+
+    /* Reading the pState values */
+    Xn1 = pState[0];
+    Xn2 = pState[1];
+    Yn1 = pState[2];
+    Yn2 = pState[3];
+
+    /*      The variables acc holds the output value that is computed:        
+     *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1]   + a2 * y[n-2]        
+     */
+
+    sample = blockSize;
+
+    while(sample > 0u)
+    {
+      /* Read the input */
+      Xn = *pIn++;
+
+      /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+      acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
+
+      /* Store the result in the accumulator in the destination buffer. */
+      *pOut++ = acc;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:    */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+      Xn2 = Xn1;
+      Xn1 = Xn;
+      Yn2 = Yn1;
+      Yn1 = acc;
+
+      /* decrement the loop counter */
+      sample--;
+    }
+
+    /*  Store the updated state variables back into the pState array */
+    *pState++ = Xn1;
+    *pState++ = Xn2;
+    *pState++ = Yn1;
+    *pState++ = Yn2;
+
+    /*  The first stage goes from the input buffer to the output buffer. */
+    /*  Subsequent numStages  occur in-place in the output buffer */
+    pIn = pDst;
+
+    /* Reset the output pointer */
+    pOut = pDst;
+
+    /* decrement the loop counter */
+    stage--;
+
+  } while(stage > 0u);
+
+#endif /*   #ifndef ARM_MATH_CM0         */
+
+}
+
+
+  /**    
+   * @} end of BiquadCascadeDF1 group    
+   */

apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c

@@ -0,0 +1,283 @@
+/* ----------------------------------------------------------------------    
+* Copyright (C) 2010 ARM Limited. All rights reserved.    
+*    
+* $Date:        15. February 2012  
+* $Revision: 	V1.1.0  
+*    
+* Project: 	    CMSIS DSP Library    
+* Title:	    arm_biquad_cascade_df1_fast_q15.c    
+*    
+* Description:	Fast processing function for the    
+*				Q15 Biquad cascade filter.    
+*    
+* Target Processor: Cortex-M4/Cortex-M3
+*  
+* 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.9  2010/08/16     
+*    Initial version    
+*    
+*    
+* -------------------------------------------------------------------- */
+
+#include "arm_math.h"
+
+/**    
+ * @ingroup groupFilters    
+ */
+
+/**    
+ * @addtogroup BiquadCascadeDF1    
+ * @{    
+ */
+
+/**    
+ * @details    
+ * @param[in]  *S points to an instance of the Q15 Biquad cascade structure.    
+ * @param[in]  *pSrc points to the block of input data.    
+ * @param[out] *pDst points to the block of output data.    
+ * @param[in]  blockSize number of samples to process per call.    
+ * @return none.    
+ *    
+ * <b>Scaling and Overflow Behavior:</b>    
+ * \par    
+ * This fast version uses a 32-bit accumulator with 2.30 format.    
+ * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.    
+ * Thus, if the accumulator result overflows it wraps around and distorts the result.    
+ * In order to avoid overflows completely the input signal must be scaled down by two bits and lie in the range [-0.25 +0.25).    
+ * The 2.30 accumulator is then shifted by <code>postShift</code> bits and the result truncated to 1.15 format by discarding the low 16 bits.    
+ *    
+ * \par    
+ * Refer to the function <code>arm_biquad_cascade_df1_q15()</code> for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.  Both the slow and the fast versions use the same instance structure.    
+ * Use the function <code>arm_biquad_cascade_df1_init_q15()</code> to initialize the filter structure.    
+ *    
+ */
+
+void arm_biquad_cascade_df1_fast_q15(
+  const arm_biquad_casd_df1_inst_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize)
+{
+  q15_t *pIn = pSrc;                             /*  Source pointer                               */
+  q15_t *pOut = pDst;                            /*  Destination pointer                          */
+  q31_t in;                                      /*  Temporary variable to hold input value       */
+  q31_t out;                                     /*  Temporary variable to hold output value      */
+  q31_t b0;                                      /*  Temporary variable to hold bo value          */
+  q31_t b1, a1;                                  /*  Filter coefficients                          */
+  q31_t state_in, state_out;                     /*  Filter state variables                       */
+  q31_t acc;                                     /*  Accumulator                                  */
+  int32_t shift = (int32_t) (15 - S->postShift); /*  Post shift                                   */
+  q15_t *pState = S->pState;                     /*  State pointer                                */
+  q15_t *pCoeffs = S->pCoeffs;                   /*  Coefficient pointer                          */
+  uint32_t sample, stage = S->numStages;         /*  Stage loop counter                           */
+
+
+
+  do
+  {
+
+    /* Read the b0 and 0 coefficients using SIMD  */
+    b0 = *__SIMD32(pCoeffs)++;
+
+    /* Read the b1 and b2 coefficients using SIMD */
+    b1 = *__SIMD32(pCoeffs)++;
+
+    /* Read the a1 and a2 coefficients using SIMD */
+    a1 = *__SIMD32(pCoeffs)++;
+
+    /* Read the input state values from the state buffer:  x[n-1], x[n-2] */
+    state_in = *__SIMD32(pState)++;
+
+    /* Read the output state values from the state buffer:  y[n-1], y[n-2] */
+    state_out = *__SIMD32(pState)--;
+
+    /* Apply loop unrolling and compute 2 output values simultaneously. */
+    /*      The variable acc hold output values that are being computed:       
+     *    
+     *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]       
+     *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]       
+     */
+    sample = blockSize >> 1u;
+
+    /* First part of the processing with loop unrolling.  Compute 2 outputs at a time.    
+     ** a second loop below computes the remaining 1 sample. */
+    while(sample > 0u)
+    {
+
+      /* Read the input */
+      in = *__SIMD32(pIn)++;
+
+      /* out =  b0 * x[n] + 0 * 0 */
+      out = __SMUAD(b0, in);
+      /* acc =  b1 * x[n-1] + acc +=  b2 * x[n-2] + out */
+      acc = __SMLAD(b1, state_in, out);
+      /* acc +=  a1 * y[n-1] + acc +=  a2 * y[n-2] */
+      acc = __SMLAD(a1, state_out, acc);
+
+      /* The result is converted from 3.29 to 1.31 and then saturation is applied */
+      out = __SSAT((acc >> shift), 16);
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+      /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+      /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      state_in = __PKHBT(in, state_in, 16);
+      state_out = __PKHBT(out, state_out, 16);
+
+#else
+
+      state_in = __PKHBT(state_in >> 16, (in >> 16), 16);
+      state_out = __PKHBT(state_out >> 16, (out), 16);
+
+#endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+      /* out =  b0 * x[n] + 0 * 0 */
+      out = __SMUADX(b0, in);
+      /* acc0 =  b1 * x[n-1] , acc0 +=  b2 * x[n-2] + out */
+      acc = __SMLAD(b1, state_in, out);
+      /* acc +=  a1 * y[n-1] + acc +=  a2 * y[n-2] */
+      acc = __SMLAD(a1, state_out, acc);
+
+      /* The result is converted from 3.29 to 1.31 and then saturation is applied */
+      out = __SSAT((acc >> shift), 16);
+
+
+      /* Store the output in the destination buffer. */
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      *__SIMD32(pOut)++ = __PKHBT(state_out, out, 16);
+
+#else
+
+      *__SIMD32(pOut)++ = __PKHBT(out, state_out >> 16, 16);
+
+#endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+      /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+      /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      state_in = __PKHBT(in >> 16, state_in, 16);
+      state_out = __PKHBT(out, state_out, 16);
+
+#else
+
+      state_in = __PKHBT(state_in >> 16, in, 16);
+      state_out = __PKHBT(state_out >> 16, out, 16);
+
+#endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+
+      /* Decrement the loop counter */
+      sample--;
+
+    }
+
+    /* If the blockSize is not a multiple of 2, compute any remaining output samples here.    
+     ** No loop unrolling is used. */
+
+    if((blockSize & 0x1u) != 0u)
+    {
+      /* Read the input */
+      in = *pIn++;
+
+      /* out =  b0 * x[n] + 0 * 0 */
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      out = __SMUAD(b0, in);
+
+#else
+
+      out = __SMUADX(b0, in);
+
+#endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+      /* acc =  b1 * x[n-1], acc +=  b2 * x[n-2] + out */
+      acc = __SMLAD(b1, state_in, out);
+      /* acc +=  a1 * y[n-1] + acc +=  a2 * y[n-2] */
+      acc = __SMLAD(a1, state_out, acc);
+
+      /* The result is converted from 3.29 to 1.31 and then saturation is applied */
+      out = __SSAT((acc >> shift), 16);
+
+      /* Store the output in the destination buffer. */
+      *pOut++ = (q15_t) out;
+
+      /* Every time after the output is computed state should be updated. */
+      /* The states should be updated as:  */
+      /* Xn2 = Xn1    */
+      /* Xn1 = Xn     */
+      /* Yn2 = Yn1    */
+      /* Yn1 = acc   */
+      /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+      /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+
+#ifndef  ARM_MATH_BIG_ENDIAN
+
+      state_in = __PKHBT(in, state_in, 16);
+      state_out = __PKHBT(out, state_out, 16);
+
+#else
+
+      state_in = __PKHBT(state_in >> 16, in, 16);
+      state_out = __PKHBT(state_out >> 16, out, 16);
+
+#endif /*   #ifndef  ARM_MATH_BIG_ENDIAN    */
+
+    }
+
+    /*  The first stage goes from the input buffer to the output buffer.  */
+    /*  Subsequent (numStages - 1) occur in-place in the output buffer  */
+    pIn = pDst;
+
+    /* Reset the output pointer */
+    pOut = pDst;
+
+    /*  Store the updated state variables back into the state array */
+    *__SIMD32(pState)++ = state_in;
+    *__SIMD32(pState)++ = state_out;
+
+
+    /* Decrement the loop counter */
+    stage--;
+
+  } while(stage > 0u);
+}
+
+
+/**    
+ * @} end of BiquadCascadeDF1 group    
+ */